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ABSTRACT 



A new type of gait and steering algorithm for use by a six-legged walking 
machine is developed and presented in this study. The spatially oriented tripod 
follow-the-leader gait is an extension of previous studies of temporal follow-the- 
leader gaits, and should prove useful for all-terrain walking vehicles, such as the 
Adaptive Suspension \"ehicle. Tractor-trailer style steering is introduced as an 
effort to tailor steering control for this type of gait. Both gait and steering 
algorithms are implemented on a color graphics computer simulation for study 
and comparison with other walking algorithms. 
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I. INTRODUCTION 



It is estimated that nearly half of the Earth’s land surface is inaccessible to 
wheeled and tracked vehicles [Ref. l]. Yet almost all of this same area can be 
successfully traversed by animals and man. This great difference in mobility has 
motivated research into the creation of a practical legged vehicle or 
ivalking machine. 

The advantages of legged locomotion can largely be attributed to the 
flexibility offered in leg placement and support. Wheeled vehicles, and to a lesser 
extent tracked vehicles, are confined to a more or less continuous, relatively flat 
and obstruction free paths along the ground. The leg’s flexibility allows the 
utilization of discontinuous support regions on the ground and the adaptation to 
terrain slope. A legged vehicle may potentially use obstructions for support as it 
climbs over those obstacles which it decides to not simply ignore. 

A second advantage of legs involves the means of obtaining traction in soft 
soil. A wheel or track creates a depression or rut from which it must continually 
work to climb out. Slippage causes the wheel or track spin, possibly digging a 
deeper hole. A leg, however, may be lifted vertically out of its depression, 
minimizing the work required. In addition, any back slip caused by the vehicle 
stepping pushes up soil behind the foot and improves traction. [Ref. 2] 



The combination of flexible coordination and increased traction provides a 
potential for greater speed and less power consumption while operating over rough 
and otherwise unsuitalde terrain. Other advantages of legged locomotion include 
possible improved comfort in ride due to the adaptive nature of legged support on 
uneven terrain, the ability to test soil conditions prior to placement of weight on 
the legs, and the relatively small footprint left in the soil. The latter may prove 
especially important for agricultural work, where the disturbance of crops is to be 
minimized, or for military vehicles navigating areas suspected of containing 
landmines. 

A. GOALS 

The purpose of this study is to explore a new type of gait and steering 
algorithm for the use of legged walking machines. The gait is a particular type of 
tripod gait, which can be considered as an extension of the temporal follow-the- 
leader gait [Ref. 3]. into the spatial domain. The steering algorithm to be 
investigated along with this style of gait borrows from the concept of driving a 
wheeled tractor-trailer vehicle. It is believed that this steering algorithm may be 
particularly well suited for the fixed foothold position requirements of follow-the- 
leader gaits. 

The machine chosen as a physical reference for the study is the Adaptive 
Suspension \’ehicle (ASV). This is a self-contained, six-legged vehicle currently 
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being evaluated at the Ohio State University for rough-terrain locomotion. The 
AS\' is a Defense Advanced Research Projects Agency (DARPA) proof of concept 
project. 

A secondary' goal is to develop a simulation model with which to study 
walking gaits and control algorithms in general for the ASV. This model is 
developed along the general lines of the simulation previously presented by Lee 
[Ref. 4], incorporating several of his modePs features, including omni-directional 
control, foot movement, and body attitude and altitude regulation algorithms. In 
addition, this simulation is to have the features of operation in either the new 
follow-the-leader tripod gait mode or in Lee's ’’forward wave” tripod gait mode, 
an enhancement of realism with a detailed color graphics display, and a menu 
system controlled with a single mouse button. 



B. ORGANIZATION 

Chapter II provides a brief overview of the previous work relating to this 
study. It includes a discussion of state of the art legged vehicles, tripod follow- 
the-leader gaits, tripod gaits, stability and simulation displays. 

A detailed discussion of the ASV simulation problem is presented in Chapter 
III. This chapter covers the configuration of the vehicle, the gait and steering 
algorithms, the simplifications assumed in the construction of the model, and the 
kinematics involved in making the ASV model walk. The final section in this 
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chapter describes the IRIS-2400 simulation hardware and software on wliich the 
model was developed. 

The simulation program's operation and functions are presented in Chapter 
ly. This includes a complete description of the operation of the program controls 
and display features. This is followed by a discussion of the means by which 
graphics are programmed on the IRIS, and by a description of the organization 
and flow of the program and its modules. 

Chapter is a review of the performance of the simulation. It includes a 
brief subjective view on the feel of driving in the two modes, 

Tlie final chapter summarizes the contributions of this study. It also contains 
comments on possible directions for future research. The program code is listed in 
the aj)pendix. 
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II. SURVEY OF PREVIOUS WORK 



A. LXTRODUCTION 

The last quarter of a century has witnessed intense efforts to build machines 
that walk. Difficulties facing researchers include the problems of controlling the 
many degrees of freedom necessary in a maneuverable leg. maintaining vehicle 
stability, creating energy efficient motion, and adapting the walking motions to 
unstructured terrain. With the advent of compact computer technology and 
computer-aided simulation and design, serious progress is now being made in 
overcoming these problems. [Ref. 5] 

Several promising working designs have emerged in the last ten years. Some 
of the most prominent include the Perambulating Whicle II (P\’II) at the Tokyo 
Institute of Technology, the Carnegie-Mellon University hexapod, the Odetics Inc. 
ODEX I, and the Adaptive Suspension Whicle (ASV) developed at the Ohio 
State University. 

The PYll is a light-weight laboratory’ model quadruped, developed in 1980. 
It features one of the first pantograph leg constructions designed specifically to 
provide simplified leg coordination and energy efficient walking. Using tactile foot 
sensors and a microcomputer mounted near the vehicle, the P\"II is able to probe 
for footholds and maneuver over obstacles. [Ref. 6] 
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The hexapod developed at the Cariiegie-Mellon University in 1982 is a self- 
contained walking machine large enough to carr\' its operator. It uses a gasoline 
engine to provide power to the legs via a set of hydraulic actuators. The 
movements of the individual legs are controlled by a series of passive hydraulic 
circuits. A built-in microprocessor interprets the driver's commands and specifies 
the correct series of leg movement patterns to be used. This arrangement frees 
the single microprocessor from the need to compute each foot trajectory. [Ref, 7] 

The ODEX I is a commercial design introduced in 1983 [Ref. 8]. An 
improved version, sometimes referred to as ODEX II, is being developed for near- 
term use in nuclear power plants [Ref. 9], The ODEX series makes use of a 
unique circular arrangement of six planar pantograph legs which allow the 
vehicles to adjust their profile for negotiation of narrow passages. The ODEX 
walking machines are directed through a radio or fiber-optic link from the 
operator to an on-board supervusorj^-level microprocessor. Each leg is controlled 
by a dedicated lower-level microprocessor which receives instructions from the 
supenusorj' level microprocessor. The new ODEX hexapod is also being equipped 
with a center-mounted arm for remote manipulation of objects, such as valves, in 
hazardous environments. [Ref. 8] 

The Adaptive Suspension \"ehicle (Figure 2.1), currently being tested at Ohio 
State University, is the first computer-coordinated legged vehicle designed and 
built for operation on natural terrain [Ref. 10]. This hexapod walking machine is 
completely self-contained, and is capable of carrjung the driver, a 500 lb. internal 
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Figure 2.1 Adaptive Suspension Vehicle 







payload, computer and control circuitr>\ and power system, in an outdoor 
environment. The ASV is the vehicle modeled in this study. A more detailed 
description of the AS\" follows in Section 3.B. 

The remaining sections of this chapter concern gaits used by walking 
machines, vehicle steering, the walking machine stability problem, and graphical 
representation of the vehicle's motion. The gait and stability sections are oriented 
towards six-legged vehicles such as the AS\'. 

B. GAIT SELECTION 
1. Definitions 

A gait is a mode of locomotion for a vehicle or animal distinguished by a 
specific pattern of lifting and placing of the feet. Gaits in general may be 
described using the event sequence notation introduced by McGhee and Jain [Ref. 
ll]. The integer i in such a sequence corresponds to the event of placing foot i on 
the ground. The lifting of the same foot is represented by the integer i f n. 
where n equals the number of legs. For the ASV, legs are numbered on the left 
side (l. 3, 5) from the front to the rear, and on the right side (2. 4. 6) in the same 
order. 

A periodic gait is one that repeats the lifting and placing pattern, and 
thus is represented by one cycle of events. A periodic gait is said to be 
nonsingular if no two of its events occur simultaneously. McGhee [Ref. 12] 
demonstrated the existence of 39.916,800 possible nonsingular periodic hexapod 
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gaits. The total number of possible hexapod gaits i^ a much larger and unknown 
number [Ref. 5]. This makes the selection of an optimum gait a very difficult 
problem. However, this thesis is concerned with a single type of gait sequence, 
the tripod sequence. These are singular gaits, in that more than one leg is placed 
at a given instant [Ref. 3]. 

A periodic gait is considered symmetrical when the stepping pattern on 
one side of the body is identical to that on the opposite side and separated in time 
by exactly one-half of the gait period [Ref. 12]. Symmetry tends to simplify the 
required leg coordination algorithms. 

The pitch of a gait is the distance between footholds, measured in body 
lengths (defined as the distance between the front and rear leg reference 
positions). Leg stroke is the linear distance the foot travels with respect to the 
body when occupying a particular foothold. Leg stroke is also expressed in terms 
of body lengths. 

2. Follow-the-Leader Gaits 

A f ollow — the-leader[FTL) gait is one in which the middle and rear legs 
on each side of the body step in the foothold locations previously occupied by the 
leading legs [Ref. 13]. Creeping FTL gaits (in which at most one leg is in the air 
at any time [Ref. 14]). were first studied by Ozguner, Tsai, and McGhee [Ref. 3]. 
Losing a temporal framework, they narrowed the number of possible FTL creeping 
gaits to 30. of which they found five to be symmetrically realizable. 
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Expanding to a spatial reference frame greatly increases the number of 



gaits in this category. A tripod creeping gait can be defined as one in which the 
legs are placed in alternating groups of three, with each group forming a tripod of 
support. In the case of the ASV. the two possible tripods are the leg sets (l 4 5) 
and (2 3 6). 

The possible distinct tripod creeping gaits can be ennumerated using an 
approach similar to that in Ozguner et ah [Ref. 3]. Choosing the placement of leg 
1 as a reference, evidently there are two possibilities for the relative ordering of 
legs 4 and 5. and three possible locations in each sequence for the insertion of the 
alternate group of legs. Furthermore, the placing of the alternate leg group (2 3 
6) can be accomplished in six distinct ways. Table 2.1 lists the 36 possible 
nonsingular placing sequences. 

It might first appear strange that the sequence ( 1 2 3 6 4 5 ) is included 
in Table 2.1. However taking two periods together, the sequence becomes 
(123645123645), which clearly shows that the placement of the legs 
occur in alternating groups of three. 

Comparing the entries in Table 2.1 to those in the table of Ozguner et ah, 
one can see that none of these sequences are listed in the latter work. This is 
because the sequences here are not temporally follow-the-leader. Yet they all are 
spatial FTL gaits. This can be seen from the gait kinematics of the example 
shown in Figure 2.2. 
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pTABLE 2.1. 


PLACING 


SEQUENCES FOR TRIPOD CREEPING 


ffi GAITS 


Gait 

Number 


Placing 

Sequence 


Tripod 2 
Insertion 
Position 


Tripod 1 Tripod 2 

Subsequence Subsequence 


1 


123645 


1 


145 


236 


2 


126345 


1 


145 


263 


3 


132645 


1 


145 


326 


4 


136245 


1 


145 


362 


5 


162345 


1 


145 


623 i 


6 


163245 


1 


145 


632 i 


7 


123654 


1 


154 


236 1 


8 


126354 


1 


154 


263 1 


9 


132654 


1 


154 


326 ; 


10 


136254 


1 


154 


362 1 


11 


162354 


1 


154 


623 


12 


163254 


1 


154 


632 


13 


142365 


2 


145 


236 i 


14 


142635 


2 


145 


263 


15 


143265 


2 


145 


326 


16 


143625 


2 


145 


362 


17 


146235 


2 


145 


623 


18 


146325 


2 


145 


632 


19 


152364 


2 


154 


236 


20 


152634 


2 


154 


263 


21 


153264 


2 


154 


326 


22 


153624 


2 


154 


362 


23 


156234 


2 


154 


623 


24 


156324 


2 


154 


632 


25 


145236 


3 


145 


236 


26 


145263 


3 


145 


263 


27 


145326 


3 


145 


326 


28 


145362 


3 


145 


362 


39 


145623 


O 

O 


145 


623 


30 


145632 


3 


145 


632 


31 


154236 


3 


154 


236 


32 


154263 


3 


154 


263 


33 


154326 


3 


154 


326 


34 


154362 


O 

O 


154 


362 


35 


154623 


3 


154 


623 


36 


154632 


3 


154 


632 
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Figure 2.2 Sequence of Stepping for a Tripod FTL 
with Pitch of 1/3 and Continuous Body Motion 



18 



It is possible to alternate body motion with leg placement in the tripod 
gait (Figure 2.3). This yields a pattern of movement that is compatible with the 
general notion of a creeping gait [Ref. 3]. It should be noted, however, that while 
such a strategy may improve the static stability of the gait [Ref. 3], the 
intermittent body motion increases the leg stroke by a factor of two, which 
greatly increases the required working volume of the legs. For this reason, and 
also because intermittent body motion slows the average vehicle forward speed, 
only the continuous body motion alternative will be considered further in this 
thesis, 

3. Singular Tripod Gaits 

Tripod gaits have proved to provide a good compromise between 
stability, maneuverability, and ease of control for the Ohio State University 
Hexapod, the ODEX I, and the ASV. For this reason tripod gaits were chosen for 
this simulation study. 

It can be seen that a tripod gait is actually a special limiting case of a 
creeping gait, where the time between the placement of individual legs within a 
tripod grouping approaches zero. Of the very large (unknown) number of gait 
sequences possible, only one can be classified as a singular tripod gait sequence. 
All differences among varieties of tripod gaits are therefore a function of 
kinematics only. 
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Figure 2.3 Sequence of Stepping for a Tripod FTL with 
Pitch of 1/3 and Alternating Body and Leg Motion 
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The most frequently used tripod gait is the limiting form of the 



forward wave gait, where the duty cycle\ id. approaches 1/2 [Ref. 4,5]. This 
study introduces the singular FTL tripod gait. Both of these gaits are 
implemented in the walking algorithms of this simulation and are described 
further in Chapter III. 

It is interesting to note that potentially the fastest forward wave tripod 
gait for the ASV is an FTL tripod gait with a pitch of one (Fig. 2.4). This of 
course can only be considered a true FTL gait if the feet are assumed to be 
dimensionless. In order to prevent the legs from interfering with one another, the 
duty cycle might be made slightly less than l/2. This would momentarily leave 
the vehicle with no supporting legs in contact with the ground. It would also 
have the disadvantage of not providing sufficient time for possible foothold 
searches by the leading legs. 

C. STEERING 

There are several different approaches to steering currently used by ground 
vehicles. The most familiar method is articulated, or automotive style steering 
[Ref. 15]. With a steering wheel, accelerator and brake, the driver of an 
automobile can directly control the vehicle's turning radius and forward velocity. 



1 



The duty cycle is the fraction of the leg cycle used for supporting the body. 
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Figure 2.4 Sequence of Stepping for a Tripod FTL 
with Pitch of 1 and Continuous Body Motion 
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Tracked vehicles, on the other hand, most frequently utilize skid steering. By 
operating the sets of tracks at differing rates, the driver controls the turning rate 
and forward velocity of the vehicle. 

Tractor-trailers use still another type of steering. Here the driver steers far 
forward of the vehicle's center of gravity. The trailer follows along in the path of 
the cab, with the steering of its center of gravity lagging behind the steering of 
the cab. Furthermore, since the trailer's wheel axle orientation constrains its 
motion, the trailer is restricted to a larger turning radius than the cab is capable 
of steering. 

Specially designed wheeled vehicles may use omni direct iorml steering [Ref. 
16]. This rarely used method allows the driver to specify turning rate and 
velocity in any horizontal direction. 

Legged vehicles have historically used similar steering approaches. McGhee 
and Iswandhi [Ref. 17], introduced a two-axis joystick control, analogous to 
articulated steering, in which one axis controlled the turning radius and the other 
controlled forward velocity. Orin [Ref. 18], applied three-axis joystick control to 
the Ohio State University Hexapod, a small laborator>^ scale walking vehicle. 
This allowed forward, lateral and rotational velocities to be specified by the 
driver, providing steering control much like that experienced in a helicopter. The 
current AS\’ uses a similar three-axis joystick control. 

Tractor-trailer style steering has not yet been applied to walking vehicles. 
This approach, which will be developed in this thesis, should give improved two- 



axis control to the driver for moving through areas of restricted maneuverability. 
The driver need only be concerned with maneuvering the front end of the vehicle. 
The body of the vehicle will follow along the proven path established by the 
footholds used by the front pair of legs. 

D. STABILITY 

The problem of vehicle balance is a vital concern for walking machines and 
has been a focus of many studies. Legged vehicles may maintain their stability 
using one of two methods, static balancing [Ref. 19] or dynamic balancing [Ref. 
20 ]. 

Static stability is attained by maintaining the vertical projection of the 
vehicle's center of gravity within the polygon defined by the supporting legs [Ref. 
5]. This method is conceptually simple. It is. however, only valid for stationary^ 
or slow moving vehicles, as it neglects the effects of inertia on stability. 

Dynamic balancing is a complex process which places fewer restrictions on 
vehicle velocity. The vehicle may be allowed to momentarily move into a 
statically unstable configuration, so long as. over time, an adequate base of 
support is provided [Ref. 20]. This is the mode of balancing normally used by 
man and most vertebrate animals. It remains an extremely complex process, 
however, which is difficult to reproduce with legged vehicles. 

This model uses only the static criteria for stability. Having the vehicle 
limited to reasonable velocities and the six legs placed in alternating tripod 
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support patterns ensures a high degree of stability. To guarantee stability, the 
usable working volume for each leg is reduced. Figure 2.5 shows a worst case 
situation demonstrating that, if the legs are confined to their respective 
constrained working volumes, the vertical projection of the center of gravity will 
always fall within the triangular pattern formed by the supporting legs. A further 
discussion of the constrained working volume can be found in [Ref. 4]. 

E. GRAPHICS 

There is a wide sj)ectrum of available options from which to choose in the 
field of graphic displays. Decisions are recpiired as to running the simulation on 
monochrome or color monitors, the type and number of dimensions for the 
projection, the use of line or solid figure representation, acceptable display 
resolution and update time, and whether to employ special hardware options. 
State of the art graphics machines also offer possibilities which include shading, 
reflectivity of surfaces, and multiple light sources. A compromise must be made 
between functionality, visual realism, and cost in order to realize an effective 
simulation. 

Past simulation models featuring the ASV [Ref. 4,21,22] have concentrated on 
basic functionality in the display. The vehicles and terrain were represented by 
simplified line drawings on a monochrome monitor. This study attempts to take 
advantage of recent developments in special hardware and software for graphics 
workstations, in order to create a more realistic and convincing simulation. It is 
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Figure 2.5 Constrained Working Volumes 
for Adaptive Suspension Vehicle 
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simulation. It is believed that the IRIS-2400 [Ref. 23] represents a good 
compromise between state of the art quality, cost, processing time and 
availability. This system was therefore selected to support the work of this thesis. 

F. SUMMARY 

This chapter provides background information on previous research leading to 
this study. Discussions include a brief survey of examples of the state-of-the-art 
walking machines, follow-the-leader and tripod gaits, vehicle steering, and the 
question of stability for walking machines. In addition, several concerns are 
expressed regarding the graphics displays used to portray the action of the 
walking vehicles. 

The following chapter contains a detailed statement of the AS\' simulation 
problem to be solved in this thesis. 
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III. DETAILED PR OBLEM STATEMENT 

A. INTRODLXTION 

This chapter is intended as a description of the nature of the simulation 
modeling problem. In it is a discussion of the configuration of the ASV, the 
mathematics governing its motion, and foothold selection and steering algorithms 
for two selected gaits. Also covered are the simplifications deemed necessary in 
the creation of the model. The final section includes a brief description of the 
modeling facilities. 

B. ASV CONFIGURATION 

The Adaptive Suspension \'ehicle (AS\') is a self-contained, six-legged 
walking machine designed to traverse uneven terrain. The operator, sitting in a 
cockpit at the front of the vehicle, controls the vehicle either at a supervusoiy^ level 
by selecting body translational and rotational velocities and allowing the vehicle 
to automatically place the feet, or by coordinating the individual legs in a 
precision-footing mode. The various control modes are discussed in [Ref, 10], 

The vehicle is equipped with an optical scanning rangefinder, mounted above 
the cab, for short-range sensing. The laser rangefinder has a range of 10 m and a 
field of view of 40 degrees on each side of the body axis, and from 15 to 75 degrees 
below the horizontal. [Ref. 10: p.S] 
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A single 900 cc four-cylinder motorcycle engine is sufficient to power the AS\' 
over sustained periods of time. This is possible due to the aluminum construction 
of the frame and legs, which make the vehicle relatively light (2700 kg) for its size 
(3.0 m height, 5.2 m length) [Ref. 10:pp. 8-10]. Power is distributed to eighteen 
hydraulic actuator pumps through an energy storage flywlieel and a series of 
shafts and toothed belts. 

Seventeen Intel 86/30 single-board computers are used for onboard processing 
and control. One l>oard is dedicated to each leg for motion control and leg sensor 
data processing. Four more boards compute stability, check actuator motion 
limits, and generate leg commands based on the operator's control inputs and the 
internal terrain model. Two additional boards are used for cockpit displays and 
controls. The terrain model is generated by the remaining five single-board 
computers using the data gathered from the optical rangefinder. [Ref. 10: pp. 8- 
10 ] 

The design of the ASV's legs features a two-dimensioned pantograph 
mounted on a baseplate hinged to the body (Fig. 3.1 and 3.2). This design offers 
the advantages of energy efficiency resulting from decoupled ground reaction force 
components, and simplicity of control [Ref. 5.6, and 24]. \"ertical and horizontal 
motion relative to the baseplate are provided by independent actuators mounted 
to the plate. Abduction and adduction motion is provided by a third actuator 
mounted on the body. 
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Figure 3.1 ASV Leg Configuration (1 of 2) 
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Figure 3.2 ASV Leg Configuration (2 of 2) 
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C. SELECTED WALKING ALGORITHMS 



1. Gaits 

The model used in this simulation study currently supports two styles of 
walking patterns. The first, closely following that used by Lee [Ref. 4] features a 
periodic tripod forward wave gait. The second is a periodic tripod 

follow — the leader (FTL) gait [Ref. 3]. Both gaits have unique advantages to 
offer the operator. 

The great advantage inherent in the forward wave gait lies in the 
maneuverability it offers the walking vehicle. The ASV. operating in the forward 
wave gait mode, is free to place its feet anywhere within a constrained working 
volume during the leg placement phase of the walking cycle [Ref. 4:pp. 59-62]. 
This freedom allows the vehicle great flexibility in range of movement: even to the 
point of permitting turning in place. 

The price for this freedom of choice for leg placement is that a foothold 
must be found and tested for each time a leg is placed on the ground. In rough or 
obscured terrain the process of probing and testing could occupy virtually all of 
the vehicle’s onboard processing capability. Thus, the vehicle's speed over ground 
could be severely limited. 

In this type of terrain the follow-the-leader gait could prove more 
advantageous. The follow-the-leader gait requires probing and testing only for 
the forward two legs. Since the following legs step precisely where the leading 
legs have gone, no further searching is needed. On difficult or dangerous terrain. 
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where extensive probing and testing of foothold is required, the FTL gait promise^ 
both greater ground speeds and more security. 

The notable disadvantage of the FTL gait is that its use drastically 
constrains the vehicle's movement. Maneuvers such as sideways stepping and 
turning in place are not possible. Turning the vehicle requires a large radius 
turning circle, similar to that needed by a tractor pulling a long trailer. 

2. Steering 

The two walking algorithms utilize different control schemes matching the 
unique gait characteristics. The forward wave gait steering mode allows the 
operator to independently specify longitudinal velocity, lateral velocity, and 
azimuth angle rate (ideally using a three-axis joystick). This allows the operator 
to take fully advantage of the gait's maneuverability. In the absence of a three- 
axis joystick for this simulation, these body translation and rotation rates are 
input through three sliding bar controls using a mouse-driven cursor on the 
display screen. 

The vehicle in the follow-the-leader gait mode, with its inherent 
restriction that the body remain between the two parallel foothold tracks, behaves 
very much like a tractor and trailer or a wagon. Just as the truck driver steers 
the cab allowing the trailer to follow in its path, the ASV operator in this mode 
steers by specifying the desired motion of the vehicle steering point. This point 
lies just behind the cockpit, mid-way between the two front legs, along the line 
joining the centers of the two working volumes. In the place of a steering wheel 
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and acceleration pedal, the operator use^5 a two-axis joystick (simulated with a 
mouse-driven cursor on a steering pad), to specify the desired magnitude and 
direction of the cockpit's relative velocity vector. 

The truck and trailer or wagon style steering commands are translated 
into desired longitudinal and lateral translation and azimuth rotation rates in 
order to maintain compatibility with the wave gait control algorithm in the 
program. This is done by first transforming the steering point (vehicle head) 
actual position and desired velocity to Earth coordinates, and 

i^dhE- ydhE- ~dhE) respectively. The desired cockpit position is 

determined by 
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where is the program display time increment. Using the desired cockpit 
position and the centroid of the middle and rear legs’ footholds (in Earth 
coordinates) the desired azimuth angle ^ is obtained. 
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The desired new position of the body’s center ^^£)- then found by 



^dE = ^dhE — 
9 



9 



VdE " ydhE - — sin^'. 
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where L is the length between the center of the working volumes of the forward 
and rear legs. 

The desired Earth translation rates and and Euler azimuth 

angle rate are determined as 
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with ^ being the current azimuth angle. These Earth and Euler rates are then 
translated to body rates ''^dzB^ 
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Foothold Selection 



indicated in the above section, a new foothold must be selected for 
each leg while operating in the forward wave gait mode. In order to maximize the 
foothold's usefulness it should be placed so that the foot remains in the 
constrained working volume during the leg's support phase for a maximum length 
of time. The optimal foothold position is determined as the ’’point on the surface 
of the constrained working volume such that [the leg’s] support trajectory is 
predicted to pass through the foot reference position” [Ref. 4: p.lOOj. To simplify 
the computation, the reference position is taken as the center of the working 
volume and a straight line is used to approximate the foot trajectory. A line is 
projected opposite to the direction of the predicted foot velocity vector at the 
reference point. The intersection of this line and the boundaries of the 
constrained working volume is then the desired foot position. Subsequent 
variation of the body velocity will alter the supporting foot trajectory, potentially 
resulting in a suboptirnal foothold. 

The follow-the-leader gait foothold selection process is much different. 
New footholds for the leading two legs are found by projecting a line along the 
velocity vector of the vehicle’s cockpit. At a set distance (l/l2 the length 
between the forward and rear hip joints), along this line, another line 
perpendicular to it is projected. This distance is one half the leg stroke of the 
vehicle while operating with a pitch of 1/3 (Figure 2.2). 
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The desired foothold is determined by where the second line intersects a line 



running through the center of the working volume parallel to the body's 
longitudinal axis (Figure 3.3). 

As a front leg abandons its current foothold, that position is recorded for 
use by the middle leg behind it. In turn, the middle leg foothold positions are 
saved for use by the rear legs. Thus, during each complete leg cycle, two new 
foothold positions are computed. This compares favorably to the six new 
footholds needed while using the forward wave gait. 
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The current program allows the driver to start operation of the vehicle 
either using the forward wave gait or the follow-the-leader gait. Once started, the 
program must be reset before switching modes. 

D. MODELING SIMPLIFICATIONS 

There are many simplifying assumptions contained within this model of the 
AS\b These simplifications were made largely in an effort to speed the 
development of such features as the follow-the-leader gait. However, the program 
framework was devised with future work in mind. Thus, wherever possible room 
was left for generalization and expansion. 

The most notable simplification in the simulation model deals with terrain. 
The ground is represented by a smooth, level, checkerboard pattern. Although 
the AS\" was developed to be able to traverse unstructured terrain, there are no 
obstacles or obstructions in the current model. Uneven terrain will require 
inclusion of an algorithm for estimating the support plane beneath the vehicle, 
foot sensors, and a new terrain display routine. A foothold probe and testing 
routine will also be needed. 

As a consequence of the use of fiat terrain, the constrained working volume 
adjustments for uneven terrain [Ref. 4: pp. 109-117] and body regulation plans for 
varying slopes [Ref. 4: pp. 87-^9] were not required. However, the basic structure 
for body attitude and altitude regulation has been retained in the program 
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modules of this thesis. 



Consequently, inclusion of sloped terrain should 



necessitate only minor program changes in these areas. 

The model contains only kinematic features of the AS\" operation. This 
means that there are no limits on vehicle acceleration imposed by the model. In 
order to prevent unrealistic performance on the part of the displayed vehicle, a 
filter was placed between the commanded inputs and the response of the vehicle. 
The kinematics and filter for simulating dynamic constraints are described in the 
section below. 



E. MODEL KINEMATICS 

The kinematics of the model of the AS\" presented here closely follow those 
developed in the computer simulation of Lee [Ref. 4]. Body motion is specified in 
terms of translation rates along the body's forward, lateral, and vertical axes (x. 
y. and z repectively) and rotation rates around these axes. The driver of the 
vehicle may directly or indirectly control the desired forward and lateral 
translation rates and the rotation rate around the vertical axis. The remaining 
three degrees of freedom are automatically regulated to maintain a desired body 
attitude and altitude with respect to the ground. 

\"ehicle^ 'dynamics are simulated through the use of a simple control filter 
inserted between the ordered rates and the actual body rates. As a result the 
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body moves with a smooth, exponential transition in response to driver and body 
regulator control commands. 

In order to realize these filtered body rates, the rates are first converted to 
earth coordinate translation rates and body Euler angle rates. Euler integration is 
then performed to produce translation distances in earth coordinates and angular 
displacement around the three body Euler axes. 

1. Coordinate Systems 

The AS\^ model makes use of two coordinate systems, earth Zj^) 

and body (j^, y^. z^^), in its calculations. The earth coordinate system is used 
wherever it is required to specify absolute position or velocities of the body, feet, 
or terrain. The earth coordinate system is defined such that the z^ axis is positive 
upward and the unit vectors x^, y^ and z^ are mutually orthogonal. 

The body coordinate system is useful in describing operator control and 
the coordination of body and legs. The origin is defined as the center of the main 
body section (excluding the cockpit). The z^ axis is projected upward through 
the top of the body, while the x^ axis is forward along the longitudinal axis and 
the y^ axis is projected to the body's left, forming the transverse or lateral axis. 

Earth coordinates are transformed to body coordinates using the 
relationship of equation 3.14. 
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( 3 . 14 ) 
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where the position vectors l] and [x^. z^. l] describe the same 

point in space in earth and body coordinates respectively and H is a 4 x 4 
homogeneous transformation matrix [Ref. 25]. The homogeneous transformation 
matrix can be derived by decomposing the transformation into a translation from 
the earth coordinate origin and a series of rotations about the Euler axes: 

The homogeneous transformation matrix represents the translation 

of the body's center to its current position d^). The first rotation about 

the body's vertical axis by the azimuth angle is represented by the matrix 
The body is then rotated about its new lateral axis by the elevation angle. and 
then about the newly formed longitudinal axis by the roll angle. 0.^ 



^ Other notations are sometimes used for these angles. For example, in Ref. 19), 0 signifies 
elevation angle and (j) denotes roll angle. 
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The four homogeneous transformation matrices [Ref. 25: p30j are; 




1 0 0 
0 10c/ 

y 

0 0 I d, 
0 0 0 1 



(3.16) 



cos 'I' .s'/ti'I' 0 0 

sin^' cos^ 0 0 

0 0 10 

0 0 0 1 

cos^> 0 sin$ 0 
0 10 0 
sin^ 0 cos$ 0 
0 0 0 1 

10 0 0 
0 COS0 siii0 0 
0 sinQ COS0 0 
0 0 0 1 



(3.17) 
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(3.19) 
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Substituting equations 3.16, 3.17. 3.18. and 3.19 into 3.15 yields: 



cos'I'cos^ cos^'sin^sinO-sin^^cos© sin^sin© -^cos^'sin^cos© d 



sin'I'cos^ cos^'cos©4-sin^'sin^>sin© sin^sin$cos©-cos'l^sin© d 



H 



— sin^ 
0 



cos^sin© 

0 



cos^cos© 

0 



(3.20) 



2. Body Regulation 

A simple control algorithm is used in this and Lee‘s model to maintain 
the attitude of the vehicle and its height above the ground. The inputs are the 
estimated support plane and the plane formed by the body's lateral and 
longitudinal axes. 

Body attitude regulation is accomplished by rotating the present body 
plane towards the desired body plane. The desired plane can be expressed as a 
function of the terrain slope and be adjusted to suit the driver^. 

The unit vector along the rotation axis, (Fig. 3.4) is given by 

= — = 1^, y" (3.21) 

I 

where and z^ are the unit normal vectors of the current and desired body 
" In the current level terrain model, the desired body plane angle is set equal to zero. 
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REPRODUCED AT GOVERrJMFNT EXPENSE 




Figure 3.4 Rotation Axis and Angle 



pianos and /r, = 0. The rotation angle. *), is given by 
-V = cos'‘(i^ • f^) 



(3.22) 



These values are used in the control function to obtain the rotation rates around 
the body’s lon^ritudinal axis, , and about its transverse axis wj . 

Body altitude is defined as the distance along the body plane’s unit 
norma, "om the estimated support plane to the body's center of gravity. 
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A map])ing function similar to that used for the body plane can be used to relate 



the desired altitude, h^. to the current terrain slope, h} 

3. Rate Computation 

The differential equation describing the simulated dynamics of the control 

filter is 

1 

y(0 = -- y(0 (3.23) 

T 

where r is the time constant of motion and y(t) is difference between the desired 
and actual position variable. Integrating both sides of equation 3.10 yields an 
exponential response 
1 

- — i 

y(0 =" e 

The control filter for altitude is then 




1 

= - [hjy-h). 



Similarly the equation producing the attitude control response is 




(3.25) 



(3.26) 



In the current model the desired height is set to a constant value 
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The rotational vector decomposes into rotation vectors about the forward and 
lateral axes, yielding: 



1 

- — k 

I 1 ' 

^1 


(3.27) 
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1 Velocity is related to acceleration using the same filter. 
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j accomplished by letting 

1 


This is 
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j y(f) ^ -^(0 

j and substituting into ectuation 3.23. yielding: 
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The accelerations for the remaining three rates are 
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Avtlority ^ • acceleration^ 

the rates are determined by equations 3.35 through 3.37, 
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where At is the time increment and is the time constant of motion. 

Body positioning in this computer model is achieved by translating the 
body center to its proper earth coordinate position and then successively rotating 
the body about its vertical, transverse and longitudinal axes. In order to do this, 
body rates are first transformed into earth coordinate translation rates and body 
Euler angle rates using the method presented by Frank and McGhee [Ref. 19]. 
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(3.38) 



Roll, elevation and azimuth angles and translation distances are then found 
through simple Euler integration: 

ynew yold ^ y ■ 
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4. Leg Kinematics 

The pantograph leg contruction yields relatively simple kinematic 

and inver>e kinematic equations. These equations differ slightly from those 
presented by Lee [Ref. 4]. This can be attributed to the use of more accurate 
dimension measurement than those assumed by Lee. 

For the front left leg (leg number one) shown in (Fig. 3.1 and 3.2), the 
foot position is given by 

ocU ^ (3.40) 

(5/^ - 4djsin0 - /^cos0 - (3-41) 

Zj. - /^sin0 ^ (5/.^ 4dj)cos0 (3-42) 

where the hip position h^, h^) and foot position (jy. z^) are given in body 
coordinates, and d^. and 0 are the joint variables. 

The inverse kinematic equations for the joint variable d^. derived from 
equation 3.40 is 
1 

^2 ’ ~ (3.43) 

5 

Rearranging and squaring both sides of equations 3.41 and 3.42 yields, 

a“sirr0 - a /^sin0cos0 ^ /^cos"0 = (y^ - h^y (3*44) 

■> o o o 

/^>in"0* a /^sin0cos0 - o'cos'0 -- [zj- h^Y (3.45) 
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whore a 



(5/., - Solving equations 3.31 and 3.32 gives. 




(3.46) 
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In addition to the joint parameters, this model requires leg upper (thigh) 
angle, a. the lower leg (shank) angle, and the knee position in body coordinates 
(j^,. 3 ^.). The thigh angle is given in terms of joint variables as 
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and the knee position as 
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(3.51) 



The knee angle is 



= tan 







(3.52) 



All six legs of the AS\" share similar geometries. The remaining 
kinematic and inverse kinematic equations can be obtained from equations 3.40 
through 3.52 with appropriate sign changes. 
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F. SIMULATION FACILITIES 



I. Hardware 

The compuTer simulation presented here is designed to run on either of 
the two Silicon Graphics. Inc. IRIS-2400 workstations currently in the computer 
graphics laboratory in the Department of Computer Science at the Naval 
Postgraduate School. The IRIS (Integrated Raster Imaging System) consists of a 
Geometry Pipeline, a general purpose microprocessor, a raster subsystem, a 60Hz 
non-interlaced high-resolution RGB display monitor and a keyboard. In addition 
each unit has been equipped with two 72 megabyte disk drives, a cartridge tape 
unit, a floating point accelerator, and a three-input mouse. The Geometry 
Pipeline is a series of ten or twelve custom \"LSI chip matrix multipliers. Lmder 
the control of the applications graphics processor, it performs matrix 
transformations, clipping and scaling of coordinates. The output is sent to the 
raster subsystem which performs functions such as filling in pixels, shading, 
depth-cueing and hidden surface removal. 

The first IRIS system is based on a Motorola MC68010 processor with 5 
megabytes of CPU memory and a 1024 x 786 x 8 bit display memory. It is also 
equipped with a digitizer tablet. The second IRIS system is a more capable 
Turbo-2400. It is based on a Motorola MC68020 processor and has 4 megabytes 
of CPU memory and a 1024 x 768 x 32 bit display memory. An Ethernet network 
connects both workstations to two \U\X 11/7S0N and one \L\X 11/750. 
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2. Software 

The IRIS Graphics Librar\^ contains a large number of graphics 
commands and utilities. This allows the user great flexibility in the choice of 
coordinate systems and display techniques. While the software is written in C. 
the graphics commands may be called in C. FORTRAN, Pascal, and Lisp. The 
code for the model presented in this study is written exclusively in C. 

G. SUMMARY 

The previous sections of this chapter outlined the physical constraints, 
simplifications, and tools used in the development of this simulation. The next 
chapter describes the operation and construction of the actual simulation 
program. 
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IV . siMULATiox proc;ram 



A. IXTRODIXJTIOX 

In this chapter the simulation program is presented. The first section consists 
of a user’s guide, with complete instructions on how to use each program feature. 
The second section introduces the working environment for graphics on the IRIS- 
2400. The final section describes the internal operation of the simulation program 
and discusses the flow through the major modules. A complete listing of the 
program is provided in the appendix. 

B. USER'S GUIDE 
1. Starting Up 

The program iralk.c is relatively simple to use. It is entirely menu-driven, 

with a single mouse button and cursor performing all selection functions. To start 

the program, type the command "u’a//:”. 

Immediately displayed on the monitor is a split screen view of the control 

panel and the ASV on its terrain (Fig. 4.1). The right half of the screen features 

a three-dimensional projection of the AS\" on a green and white checkerboard 

plane against a blue backdrop. The user's vantage point is fixed relative to the 

center of gravity of the vehicle (above and initially to the vehicle’s left side), so 

that the vehicle will continuously remain in view while walking. 
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Figure 4.1 View of Initial Screen 




The left half of the screen contains a two-dimensional representation of 
the control panel. Initially it features only the six yellow selection panels of the 
main menu on a cyan background. 

2. Menus 

A menu item is selected by placing the cursor over the corresponding 
panel and clicking the middle mouse button. Pressing the button down will cause 
the panel beneath the cursor to be highlighted in red, as a potential choice. 
Releasing the button selects the highlighted menu item. If no changes are desired 
in the current menu selection, simply move the cursor to a portion of the screen 
outside the menu selection region and release the mouse button. Selected menu 
items are highlighted in bright yellow. 

3. Forward Wave (iait 

In the forward wave gait mode, vehicle velocities are specified in terms of 
body axis translation and rotation rates. Three of these rates - longitudinal 
velocity, lateral velocity, and yaw rate, are directly controllable by the operator. 
The rates for the remaining three degrees of freedom are automatically adjusted 
by the vehicle in order to maintain proper attitude and altitude. All rates in this 
mode are defined with respect to the body's center of gravity. 

Selecting the forward wave gait panel produces a secondar>' menu 
displayed immediately below the main menu (Fig. 4.2). This secondary' menu 
contains three additional panels for use in specifying the vehicle's body rates. The 
panels are operated in the same manner as those in the main menu. To the right 
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Figure 4.2 View of Forward Wave Gait Screen 



of the menu panels are six simulated LED readouts, used for displaying the 
magnitude of the current and ordered rates. 

Releasing the middle mouse button while the cursor is inside the bounds 
of one of the secondary’ menu panels results in a sliding bar control panel being 
displayed on the left edge of the screen (Fig. 4.3). \"elocity commands are input 
by placing the cursor within the black center region of the bar control area. A 
yellow bar level indicator will rise or fall to match the cursor level, indicating the 
commanded velocity value. No clicking of the mouse button is required. To set 
the commanded input at the desired level, move the cursor to the desired height 
and then slide the cursor horizontally until it is outside the center region of the 
sliding bar panel. A red bar level indicator displays the current velocity of the 
vehicle. 

4. Follow-the-Leader Gait 

Control while in the follow-the-leader gait mode is achieved by specifying 
the desired relative velocity vector of the ASV‘s steering point. The operator, in 
essence, points the vector in the direction in which the steering point should 
travel, relative to the body longitudinal axis. As stated in the previous chapter, 
the steering is very much like that of a long tractor-trailer type of vehicle. The 
control algorithm factors in the magnitude of the desired velocity, footholds and 
current velocity and automatically regulates the body's motion. 
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Figure 4.3 View of Forward Wave Gait Screen with Sliding Bar Control 




The follow-the-leader gait control mode is invoked by using the lower left 
panel of the main menu. When this panel is selected, a white rectangular control 
area appears directly beneath the main menu (Fig. 4.4). If the middle mouse 
button is held down while the cursor is within this region, the cursor controls a 
simulated two-axis joystick. The vertical axis represents the magnitude of the 
relative velocity vector and the horizontal axis represents the direction. A solid 
yellow line is used to indicate the current joystick position, and thus the input 
values. The vehicle's actual relative cockpit velocity vector is indicated by a solid 
red line. 

5. Status and Warnings 

The status menu option exists to provide the operator with numerical 
data on leg and body position and movements. Selecting this item causes a 
yellow and black display panel to appear below the main menu (Fig. 4.5). 
Featured on this panel are the translation and rotation rates (with respect to the 
body axes), the position of the vehicle's center of gravity (in Earth coordinates), 
the vehicle's orientation (in Euler angles), the walking cycle period, the position of 
each foot (in body coordinates) and the angles of various components of the legs. 
The values are updated each display cycle. 

During the operation of the vehicle, checks are made on operating 
parameters. If a leg become positioned so that the foot is outside its 
corresponding constrained working volume, a red warning box is flashed in the 
lower left corner of the screen. Similarly, if the walking cycle period becomes too 
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Figure 4.4 View of Follow-the-Leader Gait Screen 




inuil<)iion limn 
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Figure 4.5 View of Status Display Screen 



small, a yellow warning box is displayed. In addition to the warning, a 
deceleration routine is activated to slow the vehicle until the period comes up to 
an acceptable level [Ref. 4: pp. 66-71]. 

6. Reset and Exit 

The reset option returns all vehicle parameters, including position, to 
their original values. This feature was included to save time when making a series 
of test runs. The exit option ends the program, clears the screen and returns the 
user to the current UNIX shell. 

C. GRAPHICS ON THE IRIS-2400 

Figures are displayed on the IRIS-2400 by calling a series of short graphics 
commands, called primatives. The primatives are interpreted into graphical 
displays by the software and special hardware of the IRIS system. These include 
commands for specifying color, drawing lines, circles, irregular polygons, and 
printing text characters on the screen. There are also a series of primatives 
designed to manipulate coordinate transformation matrices for the purpose of 
scaling, rotating and translating figures. 

A sequence of graphics commands may be grouped into a listing called an 
object. This object list may then be conveniently executed using a single call. 
Once created, the object list may at any time be edited as desired through the use 
of object tags. The object, in essence, functions as a reconfigurable graphics 
subroutine. 
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Objects and figures in this program are displayed in the reverse order in 
which they are called. The last object is overlaid in front (with respect to the 
viewer’s vantage point), of the previously called objects. An important exception 
to this is the treatment of the back face of polygons. The reverse side of a 
polygon, in the back face polygon removal mode, is considered transparent and is 
aiitomatically removed from the image as a hidden surface by the IRIS hardware. 
Th is feature enables the display of more realistic appearing three-dimensional 
objects. 

In the double buffer display mode utilized by this program, the special display 
memory is divided into two sets of bit plane buffers. As one buffer is having 
display data written into it, the other is used to refresh the monitor. Once the 
writing is complete, the functions of the buffers are swapped, and a new cycle of 
writing commences. This display mode provides for a smooth simultaneous 
update of the entire screen. 

D. PROGRAM ORGANIZATION 

The simulation program can be divided into three general sections; 
initialization, simulation loop, and termination. The heart of the program, the 
simulation loop, cycles through an input phase, which serv^es as the operator's 
control interface for the vehicle: the calculation phase, in which the parameters 
for the position and orientation of the AS\"'s body and legs are determined; and a 
display phase. The initialization section performs tasks, such as defining 
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coordinate systems and creating object lists, required to start up the loop. The 
termination section clears the screen and buffers in preparation for the next IRIS 
user. A flow chart featuring the program’s primary' modules is shown in Figure 

4.6. 

Since the order in which objects are called is critical in this display mode, 
special provisions are needed to create a full 360 degree viewing coverage of the 
maneuvering vehicle. Speciflcally, four separate ASV objects lists are created in 
the object construction module of the initialization section. Each object has the 
vehicle components ordered for proper viewing from one of four viewing 
quadrants. The display section therefore needs only to determine from which 
quadrant the vehicle is to be viewed and call the appropriate object. 

The simulation loop is the dominant part of the code, containing the 
overhelming majority of the program modules. The loop begins with a call to the 
driver's command interface module. This module controls the operation and 
display of the menu system, the status panel, and the sliding bar and joystick 
controls, as well as processing F.T.L. gait steering commands. The organization 
of the control module is shown in Figures 4.7 and 4.8. 
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Figure 4.7 Command Interface Module (1 of 2) 
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Figure 4.8 Command Interface (2 of 2) 




Immediately following the command module are two checks on the program 
status. If the exit option was selected in the command module, the loop is 
interrupted and the program enters the termination stage. The reset option 
causes a module call to re-initialize all working parameters. 

The calculation phase of the loop is begins with the support module, where a 
determination of the estimated support plane directly beneath the vehicle is 
made. The position and velocity of the ASV’s body is then calculated in the 
body rates module (Figs. 4.9 and 4.10). The body kinematics used in this module 
are discussed in section III.E. 

The gait period is next calculated in the optimal period module. This module 
uses a optimal period control algorithm which considers the kinematic limit of the 
supporting legs. In this algorithm, a period is calculated for each leg, based on 
the time required for its foot to reach the limits of its corresponding constrained 
working volume. The minimum of all of the supporting leg's periods is chosen as 
the vehicle's optimal period. No foot should therefore be required leave its 
constrained working volume while supporting the body. A backward gait period 
is also computed for the use of the wave gait walking in the reverse direction. 
[Ref. 4: pp. 63-69] 
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Figure 4.9 Body Rates Module (1 of 2) 
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Figure 4.10 Body Rates Module (2 of 2) 
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The decehration module checks the output of the previous module. If the 
period is below the set threshold, the vehicle is slowed. Each time the period falls 
below the minimum value, longitudinal body velocity is cut ten percent and 
lateral velocity and yaw rate are cut twenty percent. This slowing occurs in each 
pass until the walking period rises to acceptable limits. 

The leg phase module is used to update the movement phase of each leg. 
The phase, expressed as a modulo one floating point number, indicates at what 
I)oint the leg is in its cycle of supporting, lifting off from the ground, being 
transferred toward the desired foothold, and being placed onto the ground. The 
relative phases of the legs in this simulation are set to move the legs in two, 180 
degrees out of phase tripods. 

The foot trajectory module uses the leg phase information and the period in 
calculating the position of the feet relative to the body. The algorithm is shown 
in the flow chart of Figure 4.11. The transfer time is the length of time allotted 
for moving the foot from one foothold to the next. This determines the speed in 
which the transfer is made. 

The phase of the leg relative to the beginning of foot liftoff is referred to as 
the transfer phase. When the leg’s transfer phase is negative, corresponding to 
being on the ground in a supporting role, the foot's relative position is determined 
by the support trajectory module (Fig. 4.12). When the leg's transfer phase is 
greater than zero but less than the liftoff-transfer transition value, the relative 
foot position is returned by the liftoff trajectory module (Fig. 4.13). Likewise a 
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Figure 4.11 Foot Trajectory Module 
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Figure 4.12 Support Trajectory Module 
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Figure 4.13 Lift Trajectory Module 
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transfer phase value between the two transition point values yields a response 
from the transfer trajectory module (Fig. 4.14 and 4.15). and a phase value 
greater than the transfer-placement transition value yields a relative foot position 
calculation from the placement trajectory module (Fig. 4.16). 

The foothold selection algorithms contained in the transfer trajectory module 
are discussed in section III.C.3. Note that within this module the desired end foot 
position is treated differently in the follow-the-leader and forward wave gait 
modes. The forward wave gait, with its high degree of maneuverability, has a 
considerable greater probability that the projected ideal position toward the end 
of the transfer phase will be much different from that at the start. Therefore the 
desired foot position in the case of the forward wave gait is updated on each pass. 
In the follow-the-leader gait case it is only calculated during the first time through 
the the module. 

The results of the calculation phase are the position and orientation of the 
body and the relative position of each of the feet. These values are used, with the 
inverse kinematic relations derived in section III.E.4. in the display phase to 
obtain the rotation angles and translation distances required to position the 
AS\"'s component parts. 
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Figure 4.14 Transfer Trajectory Module (1 or 2) 
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Figure 4.15 Transfer Trajectory Module (2 of 2) 
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Figure 4.16 Placement Trajectory Module 
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The AS\' object lists are then edited and the updated parameters inserted 
into their corresponding rotation and translation commands. Once this is 
completed, the display calls are made for the background, the terrain object and 
then the properly ordered AS\" object. Swapping display buffers completes the 
loop. 

The AS\" simulation program presented here consists of fifteen separate files 
linked, together with the graphics, math, and standard input-output libraries, 
using the UXIX make utility. The program files and Makefile listings are 
presented in the appendix. The routines were created in a modular fashion for 
ease of development and testing and to assist in future program changes. 
Constants are grouped into a single header file walk.h, for convenient reference 
and modification. 

E. SUMMARY 

This chapter describes the AS\" simulation program. The first section is a 
guide for the operation of the program. It details the use of the menu system and 
the operator controls. The following section discusses the display of graphics on 
the IRIS-2400. The final section covers the organization and flow of the main 
routine and its modules. 
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V. miULATON PERFORxMANCE 

This chapter provides a brief review of the performance of the ASV 
simulation program. The review is largely subjective and is based on the author’s 
experience with the operation of the simulation. 

A. MODELING FIDELITY 

The image of the vehicle on the screen appears to be a reasonable likeness of 
the actual AS\\ This is believed, to a great extent, to be due to the proper 
scaling of dimensions of component parts, based on available blueprints of the 
ASV. Details such as the leg hydraulic actuator housings and the optical 
scanning radar, mounted on the cab of the vehicle, add to the visual effect. The 
color scheme of the simulation vehicle has been altered to enhance the visibility of 
the vehicle and its parts. 

The walking motion of the model is very similar to that of the real vehicle. 
This obser\'ation is based on viewing of videotapes produced at the Ohio State 
University. A notable difference is that the simulation model is perceived to 
operate at a much slower speed. A simulation time increment of 1/ 100th of a 
second yields a display time to real time speed ratio of 1:30. Operating the 
simulation with a simulation time increment much greater than 1/ 100th of a 
second to compensate for this causes problems related to the optimum period and 
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leg phase inodule:^ of the program. This leads to gross errors in the foot trajectory 
planning algorithms. 

B. FORWARD WAVE TRIPOD GAIT 

Driving in the forward wave tripod gait mode is a very simple task. Although 
a three-axis joystick would be prefered, the mouse-driven sliding bar control is 
easy to use and effective. Switching between control bars for forward, lateral, or 
rotational control can be accomplished with reasonable ease. 

The externa! vantage point of the vehicle causes very little problem for 
maneuvering the vehicle. This may change as the model’s speed increases and 
obstacles are introduced into the environment. 

Overall, maneuverability of the AS\^ in the forward wave tripod gait mode is 
clearly demonstrated with this model. 

C. FOLLOW-THE-LEADER TRIPOD GAIT 

The follow-the-leader tripod gait appears to work especially well for forward, 
straight-line locomotion. Turning, however, is extremely restricted. Preliminar\^ 
investigations indicate a minimum turning radius of 18 times the body length, 
using the constrained working volumes depicted in Figure 2.4, This is far greater 
than expected. An estimated envelope for turning, based on initial simulation 
trials, is shown in Figure 5.1. Steering commands falling outside of this envelope 
result in faults within the foot trajectory planning algorithms. These faults 
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usually occur within the first four degree^ of the turn attempt. Decreasing the 
display time interval extends the maximum magnitude of the command envelope, 
but only marginally improves the permitted relative direction command input. 

The shape of the steering envelope is rather unexpected, and as of yet, 
unexplained. Factors likely to have the greatest influence on the envelope are the 
geometry of the leg's constrained working volume and the implementation of the 
optimum period and foot trajectory planning algorithms. 

Expanding the constrained working volume to the full working volume has a 
remarkable effect on the maneuverability of the vehicle, while operating in the 
follow-the-leader gait mode. By doing so, the minimum turning radius improves 
to approximately five times the l^ody length. This indicates a great potential 
advantage in utilizing dynamic stability algorithms for future gaits. 

Overall the follow-the-leader gait and tractor-trailer steering appear to be 
successful for level, relatively obstruction-free terrain. Further research is needed 
to determine the nature of the limitations and the means to expand the vehicle's 
maneuverability while operating in this mode. 
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VI. SUMMARY AND CONCLUSIONS 



In th is thesis a tripod follow-the-leader gait class is introduced for use by six- 
legged walking vehicles. The class represents an extension of previously defined 
follow-the-leader gaits and should prove useful for legged vehicles traveling in 
rough or treacherous terrain conditions. 

A new style of steering is also developed for follow-the-leader gaits. This 
steering mode exhibits a general response similar to that found in steering a 
wheeled tractor-trailer vehicle. With this mode, the driver is concerned only with 
specifying the velocity of the front of the vehicle. The algorithm ensures that the 
body of the vehicle follows along the path of the front. 

An improved simulation model constructed to study the gait and steering 
algorithms is also presented in this thesis. The vehicle selected as a physical 
reference for the model is the Adaptive Suspension Vehicle (ASV), which is 
currently undergoing testing and development at the Ohio State University. The 
model developed is intended as a general tool for analyzing a variety of walking 
control algorithms for legged vehicles. 
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A. RESEARCH COXTRIBUTIONS 



Previous research on follow-the-leader gaits [Ref. 3], has concentrated on gaits 
that are temporally oriented. Since footholds are used by the following legs 
immediately after being abandoned by the lead leg. this produces a creeping 
motion with alternating leg and body movement. 

Extending the class of follow-the-leader gaits into the spatial domain relieves 
the requirement of immediately utilizing a foothold as soon as it is abandoned. 
This gives a greater degree of freedom to leg movement and allows the possibility 
of sinootli. continuous body motion with shorter leg stroke. 

The nature of a follow-the-leader gait greatly constrains the maneuverability 
of the walking vehicle. The vertical projection of the vehicle's center of gravity is 
required to fall within the support pattern of the legs and is therefore confined by 
the histor>' of footholds produced by the lead legs. The similarity of this problem 
to that of a trailer pulled by a tractor cab has inspired the adoption of the term 
’’tractor-trailer^' steering. With tractor-trailer style steering, the driver controls 
the path of the front of the vehicle. As long as the driver does not turn too 
sharply (possibly causing a wheeled tractor-trailer to jack-knife), the vehicle’s 
body follows along this path. 

The selection of footholds for the leading legs is based on projecting the 
relative heading vector provided by the operator. The location of recently 
abandoned footholds is retained within the control algorithm for use by the 
middle and rear legs. 
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The simulation presented in tliis study models the kinematics of the ASV. 
The model incorporates many of the simulation features presented by Lee [Ref. 4]. 
including omnidirectional control, automatic body altitude and attitude 
regulation, leg motion planning, body deceleration, and filters between the control 
inputs and reaction which provides the operator with the ’Teel” of vehicle 
dynamics. A simplified variation of constrained working volumes is also used. 

The simulation program has a modular design which creates a flexible 
environment for studying various gaits and control algorithms. The program as 
currently configured has two modes of operation. The first features a forward 
wave tripod gait with three-axis control for steering in body coordinates. The 
second mode utilizes the follow-the-leader tripod gait and two-axis control 
tractor-trailer style steering, developed in this study. The program's displays and 
controls are operated with a mouse-driven menu package using a single mouse 
button. 

The graphics presentation is greatly improved over Lee's monochrome line- 
drawing representation. Three-dimensional, solid body, color graphics are made 
possible through the implementation of the model on the special purpose software 
and hardware of the IRIS-2400 system. This provides a notable enhancement of 
realism for the vehicle simulation. 
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B. RESEARCH EXTENSIONS 



It ha> become clear, through the work of developing this study, that there are 
many dirt^ctions in which future research could be pursued. Four major areas to 
be considered for extension are: quantitative measurement of the FTL tripod gait 
performance, improvement of program features, improvement of display speed, 
and expansion of upper level control algorithms using artificial intelligence. 

Developing performance criteria for the simulated is critical if one is to 

effectively use the program as an aid for developing and evaluating walking 
algorithms for the actual machine. Initial research might well concentrate on 
measuring turning radii, steering reaction times, stability margins and basic 
parameters of mobility. 

As with any simulation model, there are many desired features which could 
be added to enhance realism. Perhaps the most important improvement for this 
type of mobile vehicle would be the inclusion of rough or uneven terrain into the 
model. Provisions were made in the development of this model for that 
eventuality. A few new algorithms, for functions such as estimating the support 
plane beneath the vehicle and adjusting the constrained working volume to 
conform to the terrain slope, will need to be written. It should be possible to 
follow the work of Lee [Ref. 4], at least initially, in improving the simulation in 
this direction. 

Because the AS\" is designed for rough terrain locomotion, developing a good 
foothold search algorithm is important. In addition, the inclusion of foothold 
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search into the simulation model would enable the FTL gait to be better 
evaluated with respect to other types of gaits in various terrain conditions. This 
would quantify the advantages of reduced foothold probing requirements for the 
FTL gait. 

This simulation could also be used to further develop steering mechanisms for 
the ASV. Most notably, the algorithm for the tractor-trailer style steering uses a 
simple method for body positioning based on the centroid of the established 
footholds. A different method for body steering which minimizes the maximum 
leg excursions might improve the vehicle's turning radius. 

Dynamic modeling and supplementing the modehs kinematics would greatly 
improve the realism of vehicle movement. Moreover, it should also notably 
increase mobility, as the vehicle would be free to utilize its leg’s full working 
volumes. This should also lead towards the development of a great number of 
new gaits, which are dynamically, but not statically, stable. 

Graphics techniques can be improved to enhance the realism of the displayed 
image. Features such as shading, depth-cueing, reflectivity of surfaces, terrain 
definition, and increased vehicle detail are all possible using current state-of-the- 
art techniques. Higher resolution monitors and an enlarged number of bit planes 
in the display hardware are also highly desirable. 

Adding additional features to the model has the decided disadvantage of 
requiring more cpu time for the simulation. As the program now exists, the 
simulated vehicle moves and reacts markedly slower than the actual vehicle. 
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Tliere are. however, ways to improve the display time for the model. The prime 
means is of course to upgrade the hardware, using newly developed and more 
capable machines. The code may also be streamlined for efficiency. Possibly 
several of the interactive features could be reduced or eliminated. 

Another possibility for improving display time is the replacement of the 
integration routines within the body kinematics section of the program with an 
incremental homogeneous transformation matrix technique used by Lee [Ref. 4]. 
Integration is used here because of the simplicity of the technique and the 
author's familiarity with the IRIS-2400 special hardware commands for rotation 
and translation. It may be that the homogeneous transformation matrix could 
also be used directly with the special hardware to provide the full transformation 
with fewer trigonometric computations. This possibility has not been investigated 
by the author. 

An interesting avenue of research to explore is to automate the upper levels of 
the control hierarchy. It may be possible to use an expert system shell running on 
a special purpose LISP machine to provide driving commands to this simulation. 
As of this writing, efforts are underway by others to establish communications 
between the IRIS-2400 system and a Symbolics 3675 LISP machine. 

Extensions to the work presented in this thesis are possible and will likely 
prove very fruitful. It is hoped that this line of research will lead to more efficient 
and practical gaits and control algorithms for legged walking vehicles in rough 
terrain. 
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APPENDIX 

PROGRAM LISTING 



This program is written for the iris-2400 
walk.c 



This is the main program for the simulation 
Relle Lyman 04 May 1987 



^include '’gl.h" 
ifinclude ’’device. h" 

^include ’’walk.h” 

^include <stdio.h ' 

^include < math h 

main() 

{ 

Object machineobject 1 leg 7 4 .textobj,vertextobj ,thighobj[7 2][4 , 
actuatorobj 7 2 4 ,shinobj 7 2 4 , walker 4 ,groundobject; 

NOTE; this program uses only elements 1-6 of arrays and vectors. 

Legs are numbered to remain consistent with original research . * 

Tag transrot tag 4 ,tr end tag 4 . legmovetag 7 |4 . 
actmovetag 7 2 4 .bodytagl 4 , 
thighmovetag 7 2 4 ,shiiimovetag 7 2j|4 ; 

Colorindex wrnask : 

int ij.k.n, 

program status. * desired status of program; RUN, HALT or RESET 
selected gait. * indicates which tripod gait is to be used * ^ 
slow flag. flag indicating deceleration is needed */ 

warning. * flag indicating supporting leg is outside of working volume */ 

leg status 7 ; status of leg (supporting, liftoff, transfer, placement) */ 

static float 

hx 7!= {0.155., 155., 0.,0.,- 155. ,-155. } . /* Leg attachment points 
hy 7 = {0,50., -50. ,50. ,-50. ,50. ,-50.}, 
hz|7j= {0,23. ,23. ,23. ,23. ,23. ,23.}, 

14 7 = {0,L4,-L4,L4.-L4,L4,-L4}; 

static .Angle theta 7' ' {0,0, 0,0, 0,0,0} . * Leg component angles * 

alpha 7 j- { 364,364,364,364,364,-364.-364}, 
gainmaT,^ {31 7, 317, 3 17, 31 7, 3 17. -317,-3 17}; 
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walk.c 



Leg component angles in radians 
Foot position in leg coordinates 



float temp. tempi .temp2. tempo, top, bottom 
alpharad 7 . 
thetarad. 
legcoord x 7 , 
legcoord y 7 , 
legcoord z 7 , 
azimuth, elev, roll, 
ordered vel mag, 
ordered vel dir, 



Temporary variables 



* / 

/ 



* Body Euler angles (rads) 

* Ordered velocity of the cockpit (magnitude) 



dl 7 , 
d2 7j. 
knee[7j|2 
foot 7 [2], 
hl4l[4], 
invh[4] |4 , 
legphase 7 






* Ordered velocity of the cockpit (direction) 
Joint variables */ 



/* Relative position of knee */ 

/* Relative position of foot */ 

* Homogeneous transformation matrix */ 

/* Inverse homogeneous transformation matrix */ 
/* Ph ase of individual legs * 
rel legphase 7j, * Phase of individual legs relative to leg one* 

period, * Period of leg cycle */ 

min period. * Minimum allowed period 

tx,ty.tz: ^ Earth coordinates of body position * 



vector rot rate. * Body rotation rates */ 

trans rate, * Body translation rates * 

ordered rate. * Ordered lateral and longitudinal translation and yaw rates * 
footpos[7j, * Position of foot in earth coordinates */ 
b footpos:7j, * Position of foot in body coordinates / 
fh 7j, /* selected footholds (earth coordinates) ^ j 

oldfh 7 ; /* old selected footholds (earth coordinates) */ 

w^ork vol cw'v[7]; /* Constrained working volumes 

plane spe; /* Estimated support plane */ 



Initialize the IRIS graphics * 

ginit() ; /* standard IRIS graphics initialization */ 

doublebuflfer() : /* double buffering mode */ 

gconfigO ; configure the IRIS (use the above commands * / 

wmask— (1 < <getplanes())-l ; /* enable all the bit planes for w’riting */ 

/ * set to 2**(getplanes() )minus one */ 
/* all bit planes on * z' 

wTitemask(wmask) ; 

backface(TRUE); set backface polygon removal on */ 
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walk.c 



qdevice(MlL)DLEMOUSE); set up the queue for the menu 

tie(MIDDLEMOrSE,MOUSEX, MOUSEY): 

rnapcolor(LTY'ELLO\\ .225,225.0): * create new colors * / 

mapcolor(\VHITEl,230,230,230); 

viewport! 1 10, 1023,0,767) ; /* set world view * 

perspective(600, (614.0/ 768). 0.0, 1023.0) : 

* make the ground * 
makeground(<^groundobject ); 

make the robot 

makewalker(machineobject ,d 1 ,d2,thet a, knee, gamma, alpha, transrot tag. 
tr end tag,w alker.leg.thighobj.actuatorobj.shinobj, 
legmovetag.t highmovetag, act movetag, shin movetag.tx.ty, tz, roll, 
elev.azimut h,hx.hy,hz,l4) ; 

Initialize the AS\' walking routine parameters. * 
init ialize(h,invh, Arot rate.tUtrans rate, (bordered rate,<fcspe. A'period, 
leg status, legphase.rel legphase, rootpos,b footpos,cw v.fh, 
oldfh.(Uselected gait, A^ordered vel mag.A^ordered vel dir, 

Amin period. A: program status,A:tx,A'ty , A'tz, A^roll,A:elev,A:azimuth): 

while(TRl E) Main program loop 

{ 

* Input the driver s commands. * 

driver command( A ordered rate.A^rot rate,A:trans rate.Arprogram status, 
b foot pos.A period. alpha, gamma, theta, A:slow flag, A^roll, A elev, 
Aazimuth,Atx,A'ty ,A:tz,A:ordered vel mag,Aordered vel dir,fh, 
A:selected gait): 

if ( program _stat us == HALT) 

Quit program. j 
break; 

} 

if (program_status = = RESET) 

{ u 

Reinitialize the ASV walking parameters, * ' 
initialize(h,invh,A’rot rate,A:trans rate,A:ordered rate, A:spe,A:period, 
leg status, legphase.rel legphase, footpos,b_footpos,cw'v,fh, 
oldfh, A^selected gait.Arordered vel mag,A:ordered vel dir, 

A' min period, A: program status, A:tx, A:ty,A:tz,A:roll, A:elev, A:azimuth) : 

} 
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* Calculate the estimated support plane. */ 

* Future revision needed for rough terrain. 
support plane(<^'spe): 



* Calculate the body rotation and translation rates. */ 
body rates(<L:rot_rate,<L'trans rate, <§jspe,h,invh, (Reordered rate, 

t X , t y , tz , rol 1 , (L: elev , <L: azi m u t h ) ; 

* Calculate the constrained working volume for the legs. */ 
con work vol(cwv,b footpos,leg status. ^k^warning); 

* Calculate the optimal period for walking. */ 

optimal period (legphase.b^footpos,<^rot rate,<Sjtrans rate,cwv, 
leg status. &:period ); 

* Decelerate if necessary. * 

decelerate(<^"trans rate.^k’rot rat e,<Sc period. slow fiag,<L:min period); 

* Calculate the phase of each leg. * ^ 
leg phase(legphase,rel legphase.<L'period); 

* Calculate the new^ position for each foot. */ 

foot _trajectory(legphase,<^ period, leg status,footpos,b footpos,fh,oldfh , 
invh,h.cw'v, (Varans rate,<^rot rate.<Cselected gait); 



* Displav the ASV on the screen. ’ 

* This section computes the new parameters to position the legs 
relative to the body, based on the relative position of the feet. 

It then check to ensure that no actuator positions exceed the limits. * j 

* Convert foot position to leg coordinates. */ 

for(i^l; i<5; — ) 

{ 

legcoord_x[i] = b footpos^i).x - hx|i]; 
legcoord_y|i] = b footpos!i].y - hy|i]; 
legcoord z[i: = b footpos[i .z - bziij; 

} 



* The foot position of the rear legs are changed to compensate for 
the ISO degree rotation used in the leg construction routine. * / 
for(i- 5; i<7; i^ — ) 

{ 

legcoord x[ii = hx i - b footpos|i\x; 
legcoord y[i' == b footposfij.y - hy i ; 
legcoord z(i — b footposd z - hz ij; 

} 
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for(i 1 , 7 : — ) 

generate required parameters dl,d2, theta 

d‘2 i - legcoord xU 5.0; 

temp - legcoord y i legcoord ydl: 

temp‘2 -legcoord z i legcoord zli ; 

dl i = -(5.0*L3-sqn(temp^temp2-L4 *L4) ) 4.0; 

tempi— 5.0*L3 — 4.0*dl i ; 

switch (i) 

{ 

case 1; 
case 3; 

case 5: tempo = tern pl"^ legcoord y + L4*legcoord z i|; 

break: 
case 2; 
case 4: 

case 6: tempo - tempi "^legcoord y i - L4*legcoord zHI; 

} 

thetarad asin(temp3 (templ*templ + L4"'L4)); 
theta i - thetarad * 573 - 0.5; 

} 



for(i- 1 ;i<7 ; i-^^) * prepare parameters for graphics 

{ * update on all 6 legs 

temp = L3^dl i ; 
tempi - d2 1 ^d2 i ^ temp* temp; 

temp2 = (Ll*Ll - L6"^L6 - tempi) (2.0*Ll*sqrt(templ)); 

alpharad i =((PI 2)-atan(d2 i]/temp)-acos(temp2)) ; 

Note; One half of a degree has been added to all angles */ 
alpha il = ( alpharad ;ip57 3^. 5); 

knee i 0' = (L2*cos(alpharad'i ) -^.5); /* relative to baseplate */ 

knee i l)= -( (L2*sin(alpharad i: )- dl i]) + 0.5):/* relative to baseplate * 

foot ^i 0 = (5.0*d2 i ^ .5); * relative to baseplate */ 

footji' 1 = -(5.0*L34 4.0"^dpi]^.5) ; /* relative to baseplate */ 
top— (knee i 0|-footd^[0 ); 
bottom = (knee|i 1-foot i l ); 

gamma i — (atan(top bottom)'‘573'f.5) ; 
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for (n ~0; n<4; n-^^) "" The walker is updated in each quadrant * 

{ 

editobjfthighobj ij|0 n ) ; * edit each leg to new * 

objreplace(thighmovetag ij 0 nj) ; ;* location 
rotate(alpha[i),'\’’) ; 
closeobjO ; 

editobj(thighobjli][l n ) ; 

objreplace(thighrnovetag[i I n]) ; 
translate(0.0,0.0,dl i ) ; 
closeobjO ; 

editobj(actuatorobj i |0j n ) ; 
objreplace(actmovetag^i [0 [nj) ; 
rotate(alpha[i|,’’'l"’) ; 
closeobjO ; 

editobjfactuatorobj iHl nj) ; 
objreplace(actmovetag;i jin); 
translate(d2 i ,0.0,-L3) ; 
closeobjO ; 

editobj(shinobj i' 0j[n ) ; 

objreplace(shinmovetag[i: On) ; 
rotate(gamma[ij/Y’) ; 
closeobjO ; 

editobj(shinobj[i| lj[n|) ; 

objreplace(shinmovetag[i| ij n]) ; 
translate((float)(knee[i| j0]),0.0,(float)kneejill 1 ) ; 
closeobjO ; 

editobj(leg i 'nj) : 

objreplace(legmovetag i][n!) ; 
rotate(theta i ,’X’) ; 
closeobjO j 

} end quadrant loop * / 

} end for leg loop i=l ... */ 

for (n=0; n<4; n*^+) 

{ 

editobj(inachineobject nj) ; 
objdelete(transrot tag[n|,tr end tag n ); 
objinsert (transrot tag[n|); 
translate(tx,ty,tz); 
rotate((int) (azimuth *573), 'Z'); 
rotate((int)(elev*573),’Y') ; 
rotate((int )( roll *573), ’X’); 

closeobjO ; 

} ^* end of quadrant loop */ 
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sel up the background 

color(BLUE). 

clear(); 

Keep the viewing relationship constant. * 
perspective(600, (61 4.0/ 7 68), 0.0, 1023.0) ; 
lookat(800.0— tx,800.0-^ ty ,550.0,tx,ty,-50.0.1100); 

" CALL THE GROUND */ 

callobj(groundobject); 

^ Display the ASV in the correct quadrant configuration 
if (azimuth 0.0) 



{ 

azimuth ^ - 2.0 "" F^I; 

} 

if (azimuth > 2.0 * PI) 

{ 

azimuth 2.0 PI; 



} 



if (azimuth 0.25* PI) 

{ 

callobj (machineobject Oj). 

} 

if ((azimuth 0.25*PI)A^ (^'(azimuth 0.75*PI)) 

{ 

callobj(machineobject 3]): 

} 

if ((azimuth ~ 0.75*PI)A'<A'(azimuth < 1.25*PI)) 

{ 

callobj(machineobject 2]); 

} 

if ((azimuth "= 1 .25*PI)<A^&:(azimuth < 1.75'^PI)) 

{ 

callobj (machineobject^ l]): 

} 

if (azimuth >= E75*PI) 



{ 

callobj (machineobject[0]); 

} 



svvapbuffers( ) ; 

} 



* end of main program loop */ 
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Clean up the screen, * 

color(BLACK) ; 
clear() ; 
swapbuffers(); 
color(BL ACK): 
clear(): 
sv\ apbuffers(): 
finish() ; 
gexilO ; 



} /* END OF MAIN PROGRAM */ 
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This is the header file for the p>rograni walk.c. 
walk.h 



Relle Lyman 
14 May 1987 






* + 5*: at i: 



*/ 



i^define BETA 0.5 

^define DELTA TIME 0.010 

^define TIME CONSTANT 10.1 
define TIME CONSTANT 2 0.25 
^^define TIME CONSTANT J 0.5 
^define FTL GAIT 1 

^define EUD WAVE GAIT 2 



^define EORWARD 1 

^define BACKWARD 0 

^define END LIFT PHASE 0.2 

^define BEGIN PLACE PHASE 0.8 
^define SUPPORTING ~ 0 

^define LIETOFF 1 

^define TRANSFER FORWARD 2 
^define PLACEMENT 3 

^define ON 1 

^define OFF 0 

^define LENGTH 310.0 

^define HALFLENGTH 155.0 



/* The length between the forward 
and aft hip joints * 

/* Half the length between the forward 
and aft hip joints 



^define FOOTLIETHEIG HT 40.0 
#defineLONG TIME 1000000 



?^define HO 160.0 

^define OUTER LIMIT 6.08 

^define INNER LIMIT 1.52 

# define RUN 0 

^define HALT 1 

f define RESET 2 

^tdefine NORMAL 0 



/* Desired body height (cm)*/ 
/* cm /sec */ 
cm/sec */ 



??define SLOW 1 

^define PI 3. 14159 



^define UP 1 

define DOWN 2 

^define IN 1 

^define OUT 0 

^define LT^'ELLOW 100 



^define WHITEl 107 

^define TEXTCOLOR BLACK 

^define NOHIGHLKHIT LTYELLOW 

^define ACTIVEHIGHLIGHT RED 
^define INACTI VEHIGHLIG HT YELLOW 
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w alk.h 



^define Ll 20.0 

^define L2 102.0 

^define L?> 24.0 

:fedefine L4 32.0 

^define L6 30.0 



struct mag in xvz * magnitude along x, y, and z axes 

{ 

float x,v,z; 

}; 

typedef struct mag in xyz vector; 

struct plane coefficients /* plane coefficients * ^ 

{ 

float a,b,c,d: 

}; 

typedef struct plane coefficients plane; 

typedef struct 
{' 

float min, 
max, 
center; 

} dimensions; 

typedef struct 
{' 

dimensions x, 
y, 
z; 

} work vol; 



typedef struct 
{’ 

int left, right, top, bottom,x0,y0,xl,yl,x2,y 2; 
char *text0,*textl.*text2; 

} menubox; 
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This is a function for the iris 2400 program walk.c. 
init.c 



X X X ^ 



Relle Lyman 27 Apr 1987 

X X X % X X xx:^XX-:^^Xxxx-:kxx*X^XX-^xxxxxx:i^XXXX*^--¥^X.xx 



^include ”gl.h” 
ifinclude ’’device. h” 

4^include alk.h” 

mil ialize(h.invh,rot rate.trans rate. ordered rate, spe, period, leg status, 
legphase.rel legphase,foot pos,b footpos.c\vv,fh,oldfh, selected gait, 
ordered_vel mag.ordered_vel dir, program status.tx,t\ .tz. roll, elev. azimuth) 

This function computes the body rotation and translation rates. 



vector *rot rate, 
*trans rate, 

* ordered rate. 

fh 7 . 
oldfh 7 . 
footpos 7 , 
b foot pos 7 ; 



' rotation rate */ 

’ translation rate 

ordered x translation, y translation, 
and z rotation rates * 

selected footholds (earth coord.) * 

^ old selected footholds (earth coord.) */ 
position of the foot in earth coord. * 

* position of the foot in body coord. * 



plane *spe: 



support plane in earth coord * 



work vol cw'v 7 ; * constrained working volume * 

float h 1 4 , ^ homogeneous transformation matrix * 

invh 4 4, * inverse of transformation matrix * 

leg phase 7 j, * phase of the phase / 
rel legphase 7 , "" phase of the leg relative to leg one 

* period, * body support period 

^tx.’^tv ,'’'tz, * position of body in earth coordinates * ^ 

'’'roll, '’'elev, *azimuth. /* body euler angles * / 

'’'ordered vel mag, * ordered velocity of the steering pt (magnitude)" 

'’'ordered vel dir; * ordered velocity of the steering pt (direction)'^/ 

int leg status 7 . * status of the leg 

^program status. * desired status of program 
'"selected gait; * type of tripod gait to be used */ 

{ 

irii i.j; 



float modulus one(); 



/* modulus one function 
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* init.c * 

* initialize the transformation matrix 

h 0] 0 - 1.0; 
h’O; 1 = 0.0; 
h O' 2 - 0.0; 
h 0) 3 = 0.0; 

h 1| 0 = 0.0; 
h ij 1 = 1.0; 
h 1| 2 0.0; 

h 1| 3 = 0.0; 

h 2; 0 = 0.0; 
h 2; 1 = 0.0; 
h 2: 2 = 1.0; 

h 2| 3 = HO; * initial height of the center of the body * 

h 3] 0' = 0.0; 
h 3j 1 ^ 0.0; 
h 3] 2 = 0.0; 
h 3l 3 1.0; 

* initialize the inverse transformation matrix * 
for (i— 0; i<3; i-;- — ) 

{ 

f*or (j = 0; j< 3; j 

{ 

invh^i]|j’ = h j ij; 

} 

invh 3|[i = 0.0; 

invh i 3 = -(h Olli *h 0] 3, - h 1 i "h;l 3 4 
h 2i(i *h 2 3 ): 

} , 

invh 3][3^ = 1.0; 

initialize the body rotation and translation rates 
rot_rate->\ = 0.0; 
rot rate->y = 0.0; 
rot rate->z = 0.0; 
trans rate->x = 0.0; 
trans rate->y = 0.0; 
trans rate->z = 0.0; 

initialize the commanded body rates */ 
ordered _r ate- >x = 0.0; translation */ 
ordered rate->y = 0.0; translation */ 
ordered_rate->z = 0.0, / rotation 

^period - LONG TIME; 

Selected gait = FWD VVAVE GAIT; 
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* mile 

* initialize the relative leg phase 
rel legphase l 0.0; 
rel legphase 2 - 0.5; 

rel legphase o — BETA; 

rel legphase 4 - BETA-0.5; 

rel legphase 5 - 2*BETA - 1.0; 

rel legphase O modulus one(2*BETA - 0.5); 

initialize the leg status and phase ^ 
for (i 1; i 7; i — ) 

{ 

leg status!! - SUPPORTING; 
legphase i rel legphase i ; 

} 



* initialize the constrained working volume for each leg 



cwv 


1 .x.min 


- 95.0; 


cwv 


1 .x.max 


215.0; 


CW V 


1 .X. center 


155.0; 


cwv 


1 1 .\ .min 


- 60.0: 


cwv 


1 1 .V max 


131.0; 


cwv 


1 .V .center 


^ 95.0; 


cwv 


1 z.min 


-240 0; 


C w V 


1 .z.max 


- 80.0; 


cwv 


1 -z. center 


- -160.0; 


cwv 


2 .x.min 


- 95.0; 


cwv 


2 ..x.max 


- 215.0; 


cwv 


2 .X. center 


r-- 155 0; 


cwv 


2 j.y.min 


q 

II 


cwv 


2 .V .max 


- - 60.0; 


cwv 


2 V. center 


- - 95.0; 



cwv 2 .z.min = -240.0: 
cwv 2,.z.max - - 80.0; 
cwv 2 . z. center - -160.0; 

cwv o .x.min ~ - 60.0; 
cwv oi.x.max 60.0; 

cwv o .X. center 0.0; 

cwv 3 .V .min 60.0; 

cwv 3 .\ .max - 131.0; 
cwv 3). V .center “ 95.0; 

cwv 3 .z.min = -240.0; 
cwv 3'. z. max - - 80.0; 
cwv 3j.z. center = -160.0; 
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CWV 


4 .x.min 




-60.0; 


cwv 


4 x.max 




60.0; 


cwv 


4 .x. center 


= 


0.0; 


cwv 


4 .y.min 




-131.0; 


cwv 


4 .y.max 




- 6U.0; 


cwv 


4 .y. center 




- 95.0; 


cwv 


4 .z.min 


= 


-240.0; 


cwv 


4 .z.max 




- 80.0; 


cwv 


4 .z. center 




-160.0; 


cwv 


5 .x.min 


- 


-215.0; 


cwv 


5 .x.max 


= 


- 95.0; 


CW'V 


|5'.x. center 




- 155.0; 


cwv 


5 .y.min 




60.0; 


cwv 


5 .y.max 


— 


131.0; 


CW V 


5 .y .center 




95 0; 


cwv 


5 .z.min 




-240.0; 


CW V 


5 .z.max 


~ 


- 80.0; 


CW V 


5 .z. center 


- 


-160.0; 


cwv 


6 .x.min 


= 


-215.0; 


cwv 


6 .x.max 




- 95.0; 


cwv 


6 .X. center 




-155.0; 


cwv 


6 .y.min 




-131.0; 


cwv 


6 .y.max 


- 


- 60.0; 


cwv 


6' .y. center 


= 


- 95.0; 


cwv 


6 .z.min 


— 


-240.0; 


cwv 


6 .z.max 


- 


- 80.0; 


cwv 6 i.z. center 




-160.0; 



* initialize the selected foothold positions * / 
fh Ij.x = cwvi 1 j.x center ^ LENGTH 12.0; 
fh 2 X = cwvj2]. X. center - LENGTH ^12.0; 
fh 3 .X = cwv|3;.x.center - LENGTH 12.0; 
fh 4j.x = cwv[4).x. center ^ LENGTH 12.0; 
fh 5 .X = cwv[,5].x. center -i LENGTH 12.0; 
fh 6 .x = cwv 6 .X. center - LENGTH/12.0; 

for (i- 1 ;i< 7:i^ — ) 

{ 

fh|i].y =: cwv i!.y. center; 
fh ij.z = 0.0: 

} 
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initialize the earth relative foot posit io^^ 
for (i l;i* 7;i - — ) 

footposii .X - fh ij.x; 
footposii y = fh i].y; 
footpos i .z — fhli .z; 

} 



initialize the old selected foothold positions * 
for (i= I :i< 7;i-r ^ ) 

{ 

oldfh i .X - fh i .x - LENGTH o.O; 
oldfh i .y - cwv!i .y. center; 
oldfh i .z - 0.0; 

} 



initialize the body relative foot positions * 
for (i- l;i<7;i-^ + ) 

{ 

b footpos i X cwv i x. center; 
b footpos i y - cwv i y. center; 
b footpos i'.z cwv i .z. center; 

} 



initialize the estimated support plane * 
spe- a - 0.0; 
spe- b - 0.0; 
spe->c - 1.0; 
spe->d = 0.0; 

initialize the ordered velocity of the steering point * 
^ordered vel mag = 0.0; 

""ordered vel dir = 0.0; 

* initialize the body attitude and position * f 
""roll = 0.0; 

"elev - 0.0; 

""azimuth = 0.0; 

"tx - 0.0; 

*ty 0.0; 

^tz HO; 

initialize desired program status 
"^program status — Rl N; 

} end of initialize 
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This IS a function for the iris 2400 program walk.c. 
driver. c 



Relle Lyman IS May 1987 

3finclude ”gl.h” 

^include ’’device. h" 

^include ’’walk.h” 
f include <sidio.h> 

^include < math.h> 



* X * * * 






/ 



rnenubox box[| = { 

0.0, 0,0,0, 0.0, 0.0,0, "not", "used", "here", 

100.200.670. 525. 120.567. 120. 597. 120. 627, "G.\IT"."W,\VE","F\VD", 

200. 300. 670.525. 220.567. 220. 597. 220.627, ".\TTITUDE"," AND", "ALTITUDE", 

300.400.670.525.320.567.320.597.320.627, " "."RESET"," ", 

100. 200. 525. 380. 120.422. 120. 452. 120. 482, "GAIT"," ","FTL", 

200. 300. 525. 380. 220.422. 220. 452. 220. 482, "REPORT"," "."STATUS", 

300. 400.525.380.320.422. 320. 452. 320.482, "PROGRA.M"," "."EXIT", 



100,200,3 1 0,230. 120,250, 120, 270, 120, 290,"REVERSE", "FOR WARD". "TRANSLATE". 
100.200,230.150.120.170.120,190,120.210."R1GHT"." LEFT". "TRANSLATE", 
100.200.150.70 120.90. 120. 110, 120.1 30."RIGHT". "LEFT". "ROTATE". 



100.200.310.2.30.120.250.120,270.120.290." "." ", 

100,200.230.150,120.170,120,190.120,210," "." ", 

100,200,150.70.120,90,120,110.120.130," "," "," "}: 

driver command (ordered rate. rot rate.trans rate, program status, 
b footpos, period, alpha, gamma. theta. slow flag, roll, ele\ . 
azimuth,tx.ty,tz. ordered vel mag. ordered vel dir. fh, selected gait) 

/* This function inputs the driver’s commands using a menu and 
the mouse. ’ 



vector “ordered 
“rot rate, 
“trans rate 
b footpoS[7 

fhl":; 



rate, ordered x translation, y translation, and z rotation rates * ' 
/* actual rotation rate vector * > 

, / "* actual translation rate vector * 

/ * position of foot in body coordinates */ 

/* selected footholds (in earth coordinates) */ 



int *program status. /* desired status of the program RUN/HALT PRESET */ 
^selected gait, desired tripod gait * 

*slow flag; /* flag indicating deceleration is required */ 

float *period, /* body support period 

*tx,*ty,*tz, /* body translation distance (Earth coord) */ 
*azimuth,*elev,*roll, /* body Euler angles * 

*ordered_vel_mag, ordered cockpit velocit) (magnitude) */ 

'’^ordered vel dir; /* ordered cockpit velocity (direction) */ 
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ihigh angle 
shin angle 
leg lateral angle 



Angle alpha 7,. 
gamma 7 , 
t beta 7 : 

Device dummy.x.y; 

static ini buttonHag - DP, pick, potenlialpick.mainmenupick, submenu, slidebar; 
int i: 

Hoal barvaiue; 
static float time; 

char str or.x 100], str oryDOO ,str orz lOOj, 

sir trx 100 ,str try lOOpstr rrz 100 ,str lime lOO); 



pushmatrix( ); 
pushviewport(): 

viev\ port (0.300,0.767); 
on ho2( 0.0, 500.0. 0.0.767.0); 

color(C3’ AX); ^ screen background color * ^ 

clear(); 

Display simulation time on top of screen 
color(TEXTCOLOR); 
time -r- DELTA TIME; 

sprintf(str time, "simulation time ^8.3f',time); 
cmov2i( 105,700); 
charstr(str time): 

if (qtest() == MIDDLEMOUSE) * Button just pressed or released ^ 

{ 

qread( A^dummy ); 
qread(<^x); 
qread( A'y); 

if (buttonflag == DOWN) /* Button was just released. * 

{ 

buttonflag ~ UP; 
if (potentiaipick == 0) 

* No change */ 

} 
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else if (potentialpick ^ 7) * Main menu chosen 

mainmenupick - potentialpick; 
pick - 0; 

pick = potentialpick; 
potentialpick = 0; 

} 

else * submenu chosen */ 

pick = potentialpick; /* no change to main menu pick */ 
potentialpick = 0; 



} 

else /* Button was just pressed. */ 

{ 

buttonflag - DOWN; 

} 

} end of qtest ' 

if (buttonflag DOWN) * Find the box over which the 
cursor lies for highlighting. 

X getvaluator(MOUSEX) ; 
y = getvaluator(MOUSEY) : 

potentialpick - 0; 

for (i= 1 :i< 7;i ^ + ) * Check the main menu. */ 

{ 

if (x < box i .right Szi: x > box i .left 
y < box i . top Sz&z y > box ij. bottom) 

{ ■ 
potentialpick = i; 

} 

} 

if (submenu 1) /* Check submenu #1. */ 

{ 

for (i = 7:i< 10;i— + ) 

if (x < boxji]. right x > boxji’.left 

V box 'i top y > box li]. bottom) 

{ 

potentialpick = i; 



} 
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drixer.c 



if (submenu = 2) * Check submenu ^1. 

{ 

for (i l();i< 13;i - ) 

i 

if (x box i .right x > box i .left 
V < box i .lop V > box i . bottom) 
'{ 

potentialpick = i; 

} 



else * button is up * / 

{ 

potentialpick 0: 



^ Display the menu. * 

for ( i I ;i< 7;i ) 

( 

if (i — potentialpick) 

{ 

color) ACTIVEHIGHLIGHT); 

} 

else if (i mainrnenupick) 

{ 

color(LN AGTI\ KHICaiLIGHT), 

} 

else 

{ 

color(NOHIGHLlGHT): 

I 

I 

rectfi(box i .left, box[i]. bottom, box|i]. right, box(i].top); 
color(TEXTCOLOR); 

recti(box ij.left, boxli .bottom, box ij. right, box ij. top); 

cmov2i(box^i^x0,box[i].y0); 

charstr(box(i].textO); 

cmov2i(box ij.xl,box[iJ.y 1); 

charstr(box[i].textl); 

cmov2i(box'i].x2,box;i .y2); 

charstr(box'i|.text2); 
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if (submenu 1) * Display submenu 

{ 

for 10;i^ ^ ) 

{ 

if (i potenliaipick) 

{ 

color(ACTIVEHlGHLIGHT); 

} 

else if (i pick) 

( 

color(INACTIVEHlGHLIGHT); 

} 

else 

{ 

color(NOHIGHLIGHT); 

} 



rectfi(box i .left, box i .bottom, box i right, box i . top); 
coior(TEXTCOLOR); 

recti(box i left, box i bottom, boxd .right, box i .top); 

cmov2i(box i .xO,box i .yO); 

charstr(boxii .textO); 

cmov2i(box i .xl,box i .yl); 

c harstr( box |i ’.text 1 ); 

cmo\2i(boxi.x2,boxi..>'2); 

charstr(box|i .text2); 



color( VV HITE); Draw LED gages. 

rectfi(200,70,400,370); 

color(BLACK); 

recli(200, 70,300, 150): 

recti(300,70,400,150); 

recli(200, 150.300,230); 

recti(300,150,400,230); 

recli(200,230,300,310); 

recli(300,230,400,310); 

recti(200, 310,300, 370); 

recti(300,310,400,370); 

color(RED); 

cmov2i(205,350); 

charstr(”ORDERED”); 

cmov2i(205,330); 

charstr(”R ATE”); 

cmov2i(305.350); 

charstr(”ACTUAL”); 

cmov2i(305,330); 

charstr(”RATE”); 
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Displav ihe parameter values. 
sprintf(str orx,”^7.2r\ordered rate- >x); 
5printf(str ory,”/t 7. 2f , ordered rate- >\ ); 
sprimf(str orz,”%7.2f’, ordered rate->z); 
sprintf(str trx.”%7.2f’.trans rate->x); 
sprintf(str tr\ ,”%7.2f ’.trans rate->y); 
sprintf(str rrz," /7 7.2f',rot rate- >z); 

c mo v2i( 205,270) ; 
charstr(str orx); 
cmov2i(205. 190); 
charstr(str ory); 
cmov2i(205, 1 10); 
charstr(str orz): 
cmov2i( 2i05,270): 
charstr(str trx): 
cmov2i(:U)5,190); 
charstr(str try); 
rmov2i(:U).VI 10); 
charstr(str rrz): 

} 

if (submenu 2) * Display submenu #2. * 

{ 

for (i 10;i<ir»;i ) 

{ 

if (i - potentialpick) 

{ 

color(.\CTlVEHIGHLlGHT); 

} 

else if (i == pick) 

{ 

color(INACTlVEHlGHLIGHT), 

} 

else 

{ 

color(NOHlGHLIGHT); 

} 



rectfi(box i left, boxli .bottom, box i right, box i lop); 
color(TEXTCOLOR); 

recti(box ii.left, boxjij. bottom, box i .right, box i top); 

cmov2i(box i .xO.boxdj.yO); 

charstr(box i .textO); 

cmov2i(box ij.x l,box[i].y 1); 

charstr(box i .text 1 ); 

c mo v 2 i ( box , i . x2 , box | i | ,y 2 ) ; 

charstr(box i .text2); 
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'' driver. c 



color(\\ HITE); * Draw LED gages. * 

rectfi(200, 70, 400.370); 

color(BL.\CK); 

recti(20().70. 300,150); 

reel i( 300.70,400, 150) : 

recti(200. 150.300,230); 

recti (300. 150.400,230); 

recli(200.230.300,310); 

recti(300.230.400.310); 

recti(200. 310.300, 370); 

recti(300. 310.400.370): 

color(RED); 

cmov2i(205,350); 

charstrC'ORDERED"): 

cmov2i(205,330); 

charstr(".'\NGLE''); 

cmov2i(305,350); 

charstr("ACTL’,AL"); 

cmov2i(305,330); 

charstrC'.ANCLE”); 



Action! ^ 

switch (pick) 

{ 

case 1: submenu 1; 

Selected gait = F\VD_\VAVE GAIT; 
break; 

case 2: submenu = 2; 

break; 

case 3: submenu = 3; 

^program status = RESET; 
break; 

case 4: submenu - 4; 

joystick(trans rate, rot rate, ordered vel mag, ordered vel dir,<Scbuttonfiag) 
steering conv(ordered rate, ordered vel mag. ordered vel dir, 
azimuth, tx,ty,fh); 

^selected gait = FTL GAIT, 
break; 
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driver. c 



ca^e o: >ubinenu 5; 

status repori(ordered rate.trans rale, rot rate, 
h foot pos. period. alpha, gamma, ihet a. 
slou Hag, roll. elev, azimuth, tx,t\ . 
iz); 

break; 

rase 6: * exit * / 

"^program status HALT; 
break; 

rase 7: bar(-200.().20().0, Aslidebar. A:barvalue,trans rate->x): 

if (slidebar — = IX) 

ordered rale- >x - barvalue; 

} 

break; 

case ^ bar(- 100.0,100 0,Aslidebar,Abarvalue,trans rate->y); 

if (slidebar — IX) 

{ 

ordered rate->v - barvalue; 

} 

break; 

ca>e 9; bar(- 1 .0, 1 .0.Aslidebar, Abarvalue.rot rate->z); 
if (slidebar IN) 

{ 

ordered rate->z barvalue; 

} 

break; 



case 10: Future expansion * 

break; 

case 11: Future expansion * 

break; 

case 12; * Future expansion 

break; 

default: color( BLACK): 

} 



popviewport ( ) ; 
popmatrix(); 

* end of driver command */ 
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driver. c 



bar(minval. ni ax val, slideba r, barv alue, ac t ual value) 
float minval. maxval/barvalue,actiialvalue: 
int ^slidebar; 

{ 

register i; 
char str 20': 
int x.y; 
static int barlevel; 

/ * Draw the sliding bar. * 

cursofT(); 

color(BLACK); 

rectfi (9,69,90,690); 

color(RED) ; 

recti(l0.70,30,670): 

for (i = 0;i< 5;i ) 

{ 

move2i(30,70 — i*150); 
draw2i(40,70 - i"*150); 
cmov2i(34,73 ^ i*150); 

sprintf(str, ”S’o6. If*', minval ^ i*(maxval-rninval) 4.0): 
charstr(str); 

} 

curson(); 

/* Check the location of the cursor. If it is inside the 
sliding bar, then calculate the value for its position. / 

X = getvaluator(MOUSEX); 
y = getvaluator( MOUSEY); 
if (10 < X X < 30 k&: 70 < y v < 670) 

{ 

barlevel = y; 

^slidebar — IN: 

*barvalue = minval ^ (maxval - minval)*(y - 70)/600.0; 

} 

else 

{ 

*slidebar = OUT; 

} 

* Draw the bar showing the actual level. */ 
color(RED); 

rectf( 15.0,70.0,25.0,(370.0— 600.0 *actualvalue/(maxval-minval))); 

/* Draw the bar showing the ordered value. * ^ 
color( YELLOW): 
recti ( 1 1,70, 29,barlevel); 

} /* end of bar / 
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^ ***»*-*:»c*:*-»*a xx*x**-'«a'**x***x:. 

joystick(trans rate. rot rate. ordered vel mag, ordered vel dir.buttonfiag) 

vector ^trans rate. * translation rates of the center of gravity in body coordinates 
*rot rate; * body Euler angle rotation rates ; 

float ’’'ordered vel mag, * ordered velocity of cockpit (magnitude) * 

^ordered vel dir; * ordered velocity of cockpit (direction) 

int ’’'buttonflag; * indicator for middle mouse button * j 

{ ^ 

int x.y.i; 



float vx.vy. velocity of cockpit in body coordinates */ 

magn.dir: * magnitude and direction of cockpit velocity vector 



* Display the steering box. 
color(F^LL E) ; 
recti(100.80,400,380): 

* Display t he grid 
for (i— 1 ;i< 1 5;i-^ - ) 

{ 

move2(90.0-ti‘20.0,80 0): 
dra\v2(90 ()-i*20.0,:580.0); 

} 

for (i= 1 ;i< 15;i^ ^ ) 

rnove2( 100.0, 80. 0^i*‘20.0); 
dravv2(400.0,80.0- i^20.0); 

} 



"" Display instructions. * ■ 
cmov2i( 1 10.65) : 

charstr(”Hold the middle button down”); 
cmov2i( 1 10,50); 

charstr(”to control the joystick”); 

* Display the current velocity of the cockpit. j 
vx = trans rate->x; 

vy rot rate->z * HALFLENGTH + trans rate->y; 
magn — sqrt(vx*vx vy*vy); 
dir = atan2(vy,vx); 
if (vx 0.0) 

{ 

dir - n.O; 

} 



linewidi h(5); 

color(YELLO\V): 

rnove2(250.0,80.0); 
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if (vx - 0.0) 

{ 

dir - 0.0: 

} 

linewidt h(5); 
coior(\'ELLO\V) ; 
move2(250. 0,80.0); 

draw2( (250. 0-400. OMir),(80.0^magn *3.0)); 

* Check the location of the cursor. */ 

X = getvaluator(MOUSEX); 
y = getvaluator(MOUSEY); 
if (*buttonflag == DOWN) 

{ 

if (100 < X X < 400 80 < v i v < 380) 

{ 

^ordered vel mag (y-80) 3.0; 

^ordered vel dir = (250-x) 400.0; 

} 

i 

f 

Display the ordered velocity of the cockpit. * / 
linewidth(3): 
color(RED); 
move2(250. 0,80.0); 

draw 2( (250.0 - 400.0 * ^ordered vel dir), (80.0 -f ^ordered vel mag * 3.0)) 
linewidth( 1 ); 

) * end of joystick 
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This is a function for the iris2400 j:)rogram walk.c 
''teering.c 



Relle Lyman 


•26 Mar 1987 


ttinclude ”gl.h*’ 
^include "device. h” 
f include "walk.h" 
^include <stdio.h * 
ft include « math.h • 





steering conv(ordered rate, ordered vel mag, ordered vel dir, azimuth, tx,ty.fh) 

This function calculates converts ordered head velocity to 
ordered bod>- translation and rotation rates. * 

Hoat ^ordered vel mag, ordered velocity of the cockpit (magnitude) 

^ordered vel dir. ordered velocity of the cockpit (direction) */ 



'^'azim 111 h . 


* body azimuth angle (radians) * 


*t\, *l \ ; 


* rurrent [)osition of the body's center of 
gravity (in earth coordinates) 


vector * ordered rate. 


ordered forward and lateral translation 


fli 7 : 


rates and azimuth angle rate * 

^ selected foothold (in earth coordinates) */ 


{ 

float hx.hy. 
dhx.dhy, 
fhcen x.fhcen y, 
dcgx,dcgy, 


current head (cockpit) position (earth coord.)*/ 
desired head position (earth coord.) */ 
foothold centroid (earth coord.) */ 

* desired center of gravity (earth coord.) */ 



dazimuth, desired azimuth angle (earth coord.) * 



diflfazm; 


difference between desired and current azimuth * / 


vector desired rate; 


/* desired earth translation rates and azimuth 
angle rate 
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steering con V 



Note: This module uses a level terrain assumption. * 

Calculate current head position (earth coordinates). * 
hx — *tx - HALFLENGTH "" cos( *azimuth); 
hy *ty ^ HALFLENGTH * sin( ^azimuth); 

* Calculate the desired head position (earth coord ). * ' 

dhx = hx -t- DELTA TIME ^ordered vel mag * cos( ^ordered vel dir -f ^azimuth) 
dhy = hy DELTA TIME * "ordered vel mag * sin( ^ordered vel dir - ^azimuth) 

* Calculate the foothold centroid. (Forward gaits only) */ 
fhcen X — (fh Sj.x^fh 4j.x-rfh:5j.x^fhj6j.x)/4.0; 

fhcen_y - (fh o.y-^fh 4j.y^fhj5].y — fh[6j.y)/4.0; 

Calculate the desired azimuth angle. */ 
dazimuth = atan‘2( (dhy-fhcen y),(dhx-fhcen x)): 
diffazm = dazimuth - ^azimuth: 

Adjust the difference to a value between pi and -pi. * 
if (diffazm < 14 159) 

{ 

diffazm -h - 6.2831853; 

} 

if (diffazm > 3 14159) 

{ 

diffazm 6.2831853: 



Calculate the desired center of gravity. 
dcgx = dhx - HALFLENGTH " cos(dazimuth); 
dcgy = dhy - HALFLENGTH * sin(dazimuth); 

* Calculate the desired rates (earth coord.). */ 
desired rate.x = (dcgx - *tx) DELTA TIME; 
desired~rate.y = (dcgy - *tyj) DELTA^TIME; 
desired rate.z = diffazm /DELTA TIME: 

/* Convert to body translation and rotation rates. */ 
ordered rate->x cos( ^azimuth) * desired rate.x 

- sin( "azimuth) " desired rate.y; 
ordered rate->y - cos( ^azimuth) * desired rate.y 

- sin( '^'azimuth) * desired rate.x; 
ordered rate->z = desired rate.z; 

} " end of steering conv */ 
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This is a routine for the iris-2400 program walk.c. 
status.c 



This routine creates a status report to be displayed 
on the viewing screen beneath the AS\\ 

Relle Lyman 27 Mar 1987 



^include ”gl.h” 
^include "device. h” 
^^include "walk.h" 



stalus report(ordered rate,trans rate. rot rate, b^^footpos, period. alpha, gamma, 
theta, slow flag. roll, elev,azimuth,tx,ty,tz) 



int ’'slow flag; ^ flag indicating deceleration is needed * 

Angle theta 7 . * leg component angles * 

alpha 7 . 
gamma 7 ; 



float ^period, period of leg cycle 

^tx.^tvTtz, * position of body in earth coordinates * 
*roll.*elev, ’'azimuth; * body Euler angles * 



vector *rot rate, , body rotation rates "" 

'trans rate, * body translation rates 

’^ordered rate, ’* ordered lateral and longitudinal and va\\ rates * 
b footpos 7 ; * foot position in body coordinates 

int i.k: 



char str fpx 7 100 ,str fpy[7j|l00],str_fpz 7j[l00], 
str orx 100 .str ory|l00,str orz lOOj, 
str trx 100 ,str try 100 ,str_trz|l00., 
str rrx 100|,str rry 100 ,str rrz lOO , 
str alpha 7 100j,str gamma 7 !l00|,str theta 7 100], 
str period 100], str slow 100 , 
str tx 100|,str ty l00|,str tz^lOO , 
str roll 1001, str azimuth;100 ,str elevjlOOi; 
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slat US. c 



sprintf(str or.x.”^^^ 7. 2P', ordered rate->x); 

sprintf(str ory."^7 7.2r',ordered rale->y); 

sprintf(str orz. "^^7. 2P', ordered rate->z): 

sprint f( sir trx."?T7.2r’,trans_rate->x); 

sprintf(str try."^7.2r',trans rate->y); 

sprintf(str trz,"%7.2f”.trans rate->z); 

sprintf(str rrx/’^7.2r',rot rate->x); 

sprintf(str rry .”^7.2f",rot rate->y); 

sprintf(str rrz,”%7.2r’.rot rate->z); 

sprintf(str per iod." ^^9. 5f”.* period ) ; 

sprintf(str tx.*'^7.2r',*tx); 

sprintf(str t> .”^7.2r', *ty); 

sprintf(str tz,’'Po7.2r'.*tz); 

sprintf(str roll,"^7d”,( (int) (*roll * 573.0))); 

sprintf(str azimuth, "^7d",((int) (*azimuth * 573.0))); 

spnntf(str elev,"^7d”,( (int ) (*elev * 573.0))); 

for (k l;k<7;k — ^) 

{ 

sprintf(str fpx k ,'’%7.2f',b footpos kj.x); 
sprintr(str_fpy k .”^7.2rVb footpos kj.y); 
sprintf(str fpzjk ,'’^^7.2f’.b footposlk .z); 
sprint f(str_alpha(k ,"^7d’Valpha k ); 
sprintf(str_gamma k , ”%7d’', gamma k ): 
sprintf(str theta k\'’/o7d”, theta k ); 

} 



pushmatrix(); 
viewport(0.400,0,767); 
ortho2(0.0, 400.0, 0.0, 767.0); 
coIor(BLACK); 
rectfi(10. 10, 400.370); 
color(YELLOW); 
rectfi(20.20,390,360); 
coIor(BLACK); 
cmov2i(220,340): 
charstr(”X”); 
cmov2i(280,340); 
charstr(” Y"); 
cmov2i(340,340): 
charstr(”Z”): 

cmov2i(30,325); 
charstr(”ordered rate"); 
cmov2i(30,310); ~ 
charstr("trans rate"); 
cmov2i(30,295); 
charstr("rot rate"); 
cmov2i(30,280); 
charstr( "position"); 
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cmov2i(2 10,265): 
charst.rC'ROLL'’); 
cmov2i(260,265) ; 
charstr(’’F^LE\'.”) ; 
cmov2i(3 10,265) : 
charstrC’AZIMlTH"); 

cmov2i(30,250) ; 

charstr( '’current attitude"); 

cinov2i(30.235); 

charstr( "ordered attitude"): 

cmov2i(30,210); 
charstr( "period"): 

if(*slow flag == SLO\\ ) * moving too fast * 

cmov2i(750,220); 
color(RED): 
charstr("TOO FAST"); 
coIor(BLArK); 



cmov2i(30, 185): 
charstr(".\ ft pos (1-3)"): 
cmov2i( 1 10.170): 
charstr("(4-6)" ): 
cmov2i(30. 155); 
charstr("y ft pos (1-3)"); 
cmov2i( 1 10, 140) ; 
charstr("(4-6)" ) ; 
cmov2i(30, 125): 
charstr("z ft pos (1-3)"); 
cmov2i( 1 10, 1 10); 
charstr("(4-6)"); 
cmov2i(30,95): 
charstrC’ALPHA (1-3)"); 
cmov2i( 1 10,80); 
charstr("(4-6)" ); 
cmov2i(30,65): 
charstr("GAMMA (1-3)"); 
cmov2i( 1 10,50); 
charstr( " ( 4-6)" ): 
cmov2i(30,35): 
charstr("THETA (1-3)"); 
cmov2i( 1 10,20); 
charstr(" (4-6)"); 
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cmov2i( 1 80,325) ; 
charstr(str orx); 
cmov2i(240,325); 
charstr(str ory); 
cmov2i(300,325); 
charstr(str orz): 

cniov2i( 180,310); 
charstr(str trx); 
cmov2i(240,310); 
charstr(str_tr> ): 
cmov2i(300,310): 
charstr(str irz); 

cmov2i( 180,295); 
charstr(str rrx); 
cmov2i(240,295) ; 
charstr(str rry); 
cfnov2i(300,295): 
charstr(st,r rrz); 

cmov2i( 180,280) : 
charstr(str tx); 
cmov2i(240,280) ; 
charstr(str ty); 
cmov2i(300,280); 
charstr(str tz); 

cmov2i( 180,210); 
charstr(str period); 

cmov2i(l80,250); 
charstr(str roll); 
cmov2i(240,250); 
charstr(str elev); 
cmov2i(300.250) ; 
charstr(str azimuth); 
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^ statu^^.c 



for (i ' 1 4 . 1 — -r ) 

{ 

k- llO^i'70; 
cmov*2i(k. 185); 
charstr(str fpx i ); 
cmov2i(k. 170); 
charstr(str fpx i + 3 ); 
cmov2i(k, 155); 
charstr(str fpy |ij; 
cmov2i(k. 140); 
charstr(str fpy i-^3!); 
cmov2i(k, 125); 
charstr(str fpz i:); 
cmov2i(k. 1 10); 
charstr(str fpz i^3 ); 
cmov2i(k.95); 
charstr(str alpha 1 ); 
cmov2i(k,80); 
charstr(str alpha i^3 
cmov2i(k.65); 
charstr(str gamma i ) 
cmov2i(k,50); 
charstr(str gamma i — 
cmov2i(k,35); 
charstr(str theta ij); 
cmov2i(k.20); 
charstr(str theta i^3 ) 

} 



popmatrix(); 



This is a function for the iris 2400 program walk.c. 
support. c 



Relle Lyman 21 Aug 1986 



^include ''gl.h" 

^include "device. h*' 

^include "walk.h" 

^include <stdio.h> 

^include <rnath.h> 

support plane(spe) 

* This module will compute a new estimated support plane based on 
the position of the supporting legs. As a temporary measure it 
is assumed the support plane is flat and at "sea level" (i.e. 
z = 0 ). The equation for the plane is Ax — By^Cz-^ D^O. */ 

plane ""spe; ^ estimated support plane in earth coordinates * 



spe‘>a = 0.0: 
spe‘>b = 0.0; 
spe->c — 1.0; 
spe->d = 0.0; 

} 
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This is a function for the ins 2400 program walk.c. 
bod\ rates. c 



Relle Lyman 



19 Apr 1987 






^include ”gl.h” 

^include "device. h" 

^include "walk.h" 

^^^include <stdio.h> 
itinclude < math.h 

bod\ rates(rot rate.trans rate.spe.h,invh. ordered rate,tx,t\ ,tz, 
roll. elev, azimuth) 

^ This function computes the body rotation and translation rates. * 



vector ^rot rate. rotation rate ^ 

*trans rate, ’ translation rate 

^ordered rate: ordered x translation, y translation, 

and z rotation rates 



plane *spe; * supp>ort plane in earth coord * / 

float h 4 |4), * homogeneous transformation matrix * 

invh 4 41, * inverse transformation matrix 

*tx,*ty,*tz, * position of body in earth coordinates 
*roll,*elev.*azimuth; * bodv Euler angles / 

{ 

ini i.j; 

float eta, * body plane attitude vvrt earth p)lane 
eta d, * desired body plane attitude 
height, distance form CG to est. support plane * / 
height d, * desired height 

gamma. "" angle between desired and present body unit normal vectors * 

kx, * X component of rotation unit vector in body coord */ 

ky, * y component of rotation unit vector in body coord */ 

ka, control law gain * 

a,b,c, i* body plane desired unit normal in body coord * 
length, * rotation vector normalizing factor 
celev,selev,telev, sine. cosine. tangent of elev 

crolLsroll.cazim.sazim, * sin and cos of roll and azimuth * 
m; 



plane sp)b: support plane in bod> coordinates ^ 

vector db unit norm, "" desired body plane unit normal in earth coordinates */ 
trails rale e, * Translation rates in earth coordinates 
rot rate e; Rotation in body Euler rates / 
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"" body rates. c ^ 

Calculate the body plane attitude, (level ground assumption!)* 
eta “ 0.0; 

Calculate desired body plane attitude (level ground assumption!)* 
eta d = eta; 

* Calculate the desired body clearance, (level ground assumption!)* 
height d — HO: 

Calculate the support plane coefficients in body coordinates. * 

* jspbj T = jspe T * h "" 

spb.a = sf)e->a * h[0][0) -r spe->b * h lj[0 t spe->c * h,2 [O + spe->d * h 3j[0 

spb.b = spe->a * hlOjjl) ^ spe->b * h lj[l -i- spe->c * h 2 ll -H spe->d * h 3 1 

spb.c = spe->a * h 0)i2^ — spe->b * h l|t2 — spe->c * h‘2 [2 + spe->d h 3 2 

spb.d = spe->a * h 0 3j — spe->b * h 1 [3 ^ spe->c * h 2 ?> + spe->d * h 3 3 

* Height of body CC above support plane * / 
height = spb.d: 

Calculate desired unit normal for the body plane in earth coord. * 
rn - sqrt(spe-:>a spe->a -t spe->b * spe->b); 

"" check for division by zero */ 
if (m>0.0) 

db unit norm..x spe->a sin(eta d) / m; 
db unit norm.v — spe-: >b * sin(eta d) / m; 

} 

else 

{ 

db unit norm.x = 0.0; 
db^unit norm.v = 0.0; 

db unit norm.z = cos(eta d); 
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* body rates. c 

* Transform the desired unit normal vector of the body p>lane 
from earth to body coordinates. a,b,c T~ invhr*b unit norm 

Note: invhr is the inverse of the rotational transformation 
submatrix (first three rows and columns of h). ^ 
a invh 0 iO *db unit norm.x - invh 0 1 *db unit norm.y *- 
invh 0 2 *db unit norm.z; 

b — invh 1 0 *^db unit norm.x — invh 1 1 '*'db unit norm.y 
invh 1 2 "*db unit norm.z; 

c = invh 2 0 "^db unit norm.x + invh 2}'l *db unit norm.y 
invh 2 2 *db unit norm.z; 

* Control law^ yielding an exponential response 

d gamma'dt = -ka * gamma */ 

ka = 1 TIMECONSTANT 1; 
gamma = acos(c): 
length — sqrt(a/^a ^ b*b); 
if (length < .00001) 

{ 

kx - 0; 
kv “ 0; 

1 

else 



components of rotation unit vector in body coordinates 
kx — -b length: 
kv - a lengt h: 

} 



Calculate rotation and translation rates * 
trans rate->z — -ka ^ (height d - height): 
rot rate->x = -ka kx * gamma; 
rot rate->y = -ka * ky * gamma: 

* Rate = dt * acceleration — old rate 

trans rate->x = DELTA TIME * (ordered_rate->x - trans_rate->x) 
TI.ME CONSTANT 2 ^ trans rate->x; 
trans rate->\ = DELTA TIME * (ordered rate- >y - trans rate->y) 
TIME CONSTANT 3 - trans_rate->y; 
rot rate- z = DELTA TIME * (ordered rate->z - rot rate->z) 
TIME CONSTANT 3 ^ rot rate->z; 

* Conversion to Earth coordinate translation rates. * 

croll = cos(*roll): 
sroll sin( ’*'roll); 
telev = tan('"elev); 
celev = cos( ^elev); 
selev = sin( "*^elev); 
cazim “ cos(*azimuth); 
sazim " sin (^azimuth); 
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bod\ rates. c 

trans rate e.x trans rate->x * crolPcazim — 

trans rate->y * (cazim*sroll*selev - sazim*celev) - 
trans rate->z * (sazim^selev cazim*sroll*celev); 
trans rate e.y = trans rate->x * crolPsazim 

trans rate->y (sazim*sroll*selev — cazini*celev) i 
trans rate->z * (cazim'^'selev sazim*sroll*celev) ; 
trans rate e.z = -trans rate->x sroll ^ 
trans rate->y croll*selev — 
trans rate->z cazim*celev; 

Conversion to body Euler rates / 
rot rate e.x — rot rate- x ^ rot rate- >> * telev * sroll -I- 
rot rate->z telev croll; 

rot rate e.y = rot rate->y * croll - rot rate->z * sroll; 
rot rate e.z - rot rate->y sroll ' celev 
rot rate->z * croll / celev; 

Integration routine */ 

""tx trans rate e.x DELTA TIME: 

*ty trans rate e.y * DELTA TIME; 

*tz trans rate e.z * DELTA TIME; 

*roll rot rate e.x * DELTA TIME; 

*elev -\-= rot rate e.y DELTA TIME; 

^azimuth rot rate e.z * DELTA TIME; 

* L^pdate the H matrix * ' 

croll = cos(*roll); 
sroll = sin(*roll); 
telev = tan(*elev); 
celev = cos(*elev); 
selev = sin(*elev); 
cazim = cos(’^ azimuth); 
sazim = sin(*azimuth); 
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bodv rales. c 



hjOilO 




crolPcazim; 


h!o;ii 




cazim^srolPselev - sazinPcelev. 


h o’;!2 


— 


sazim*selev ^ cazinPsrolPceiev; 


h 0 |3 


- 


*lx: 


h l'|0^ 


- 


sazim'^croll: 


h 1 |1 




cazim^celev -r sazinPsrolPselev; 


h ll|2’ 




sazinPsrolPcelev — cazim*selev; 


h 1 l’: 


- 


*ly 




hi2jjo 




-sroll: 


h ;2'|1. 




croll*selev; 


h'2 2 


- 


croll*celev; 


h 2 !3^ 




*tz 




h 3 jo; 




0.0 




h 3 1 




0.0 




h 3 '2 




0.0 




h 3 [3 




1.0 





* inverse homogeneous transform matrix ^ 

for (i -0; i< Z; i ) 

{ 

for (j-~0; j' 3; j-*) 

{ 

invh i j - h ; 

j 

invh 3 i — 0.0; 

invh 1 3: - -(h 0"i *h 0!|3 - h l]|i *h l]l3 ■ h|2| i *h;2ii3 ); 

} 

invh;3 '3 10; 

} end of bod\ rates 



% 
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This is a function for the iris 2400 program walk.c. 
con work vol.c 



Relle Lyman 19 Apr 1987 



^include ”gl.h'’ 
^include "device. h" 
ffinclude "walk.h" 
^include <stdio.h> 
^include <math.h> 



con work vol(cwv.b footposdeg status, warning) 

This module will compute a new constrained working volume for 
improved stability on rough terrain. Currently all values are 
set for a perfectly fiat support plane. Dimensions are 
expressed in cm. * 



work vol cw'v[7 ; T* constrained working volume in body coordinates 
vector b footpos 7 ; foot position in body coordinates * 



int 



{ 



leg status 7|.C^ status of leg (supporting, liftoff, transfer, 
placement) * 

^warning; flag indicating supporting leg is outside of 

the working volume * 



int i; 



^warning = OFF. 

for (i=l:i<7;i^-f ) /* check each leg */ 

{ 

if (leg status i == SUPPORTING) 

{ ■ 

if ((b footpos i].x < cwv[i].x.min): I 
(b footposli'.x > cw'v[i].x.max)| 

(b_footpos|i .y < cw^v[i].y .miii)| | 

(b footpos'i .y > cw’v[i].y.max)l 
(b footpos'i .z < cwv[i i.z.min) 

(b footpos'i .z > cwv[il.z.max)) outside limits * 

{ 

^warning = ON; 

} 

} 

} 
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con work vol.c 



if (^warning == OX) 

{ 

pushmatrix(); 

pushvicwport(); 

viewport (0,1 30,0,80); 

ortho2(0. 0,130.0.0.0.80.0); 

color(RED); 

rectfi( 10,10. 130.70); 

color(BL ACK); 

cmov2i(10,30); 

charstr(” OUTSIDE CWV”); 

pop view port (); 

popmatrix(); 

} 
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This is a function for the iris24()() program walk.c. 
opt period. c 



Relle Lyman 29 Apr 1987 



include "gl.h" 
^include "device. h” 
^include "vvalk.h" 
^include <stdio.h > 
f include * math.h> 



optimal period(legphase,b footpos,rot rate,trans rate, cwv, leg status, period) 
This function computes the optimal period for walking. ! 



vector "*rot rate, 
"^trans rate, 
b footposiT ; 



' body rotation rate * 
body translation rate 
position of foot in body coordinates * 



work vol cwv 7 ; 



* constrained working volume "" 



float legphase 7 . 
* period: 



* phase of leg 
/ * body support period * 



int leg status^7); /* status of leg 0 = supporting */ 



{ 



vector b footvel; 



foot velocitv 



float fx.fvTz, * foot position 

tmin, /* minimum temporal kinetic margin */ 

tx,ty,tz, /* temporal kinetic margins * 
d, /* distance to cw'v limit * 

speed, /* magnitude of body velocity */ 

fs period, /* forward support period * 
bs period, /* backw-ard support period * ' 
min fs^period, minimum forward support period */ 
min_bs period, /* minimum backward support period */ 
fs phase, /* forw'ard support phase */ 
bs phase. * backward support phase */ 
mvx,mvv,mvz; 



int i. 

minleg: 

static int gait = FOR\\ ARD; W ave gait direction 
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optimal period 

* Initialize variables 
trnin ~ LON(i TIME; 
niin fs period - LON(^ TIME; 
min bs period LONCi TIME; 

For each leg compute the maximum instantaneous support period, 
for (i 1 ; i< 7; i t t ) 

{ 

* Support check * 
if (leg status i| == SUPPORTING) 

{ 

* Compute foot velocity. 

b footvel.x = “(trans rate->x)-^ (rot rate->z b footpos ij.y) 
-(rot rate->y * b footpos i .z): 
b footvel.y = -(trans rate- > y)-(rot_rate->z b footpos i .x) 

-^(rot rate->x * b footpos|i .z); 
b footvel.z = -(trans rate- z)- (rot rate->y * b footpos i x) 
-(rot rate->x b footpos i .y); 

* Check to see if fool is in cwv. *. 
fx - b footpos.i x: 

f) - b foot pos i y; 
fz b footpos i .z; 





cu v i 


x. min) 


(fx 


cwv 


i .x.rnax) 




(fy- 


cwv 1 


.y .min) 


1 


cwv i 


1 y.max) 




(fz< 

tmin 


cwv i .z.min) 
- 0.1; 


(fz • 


cwv i 


z.rnax)) 


* outside cwv 



else 



* Compute distance to x limit and the temporal 
kinetic margin in the x direction. */ 
if (b footvel.x > 0.0) 

{ 

d cwv i .x.rnax - fx; 
tx = d b footvel.x; 

} 

else if (b footvel.x < 0.0) 

{ 

d = fx - cwv i].x.rnin; 
t\ — d -b fooivel.x; 

} 

else 

{ 

tx LONG TIME; 

} 
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optimal period 

Z’" Check for minimum value. */ 
if (tx<tmin) 

{ 

tmin - tx: 

} 

Compute distance to y limit and the temporal 
kinetic margin in the y direction. */ 
if (b footvel.y > 0.0) 

{ 

d — cwvdi].y.max - fy; 
ty = d / b footvel.y; 

} 

else if (b footvel.v < 0.0) 

{ 

d = fy - cwvii .y.min; 
ty ^ d / -b footvel.v; 

} 

else 

{ 

ty = LONG TIME: 

} 

!* Check for minimum value. */ 
if (tv<tmin) 

{ 

tmin — tv: 

} 



■ * Compute distance to z limit and the temporal 
kinetic margin in the z direction. * / 
if (b footvel.z > 0.0) 

{ 

d = cwv[i].z.max - fz; 
tz = d / b footvel.z; 

} 

else if (b_footvel.z < 0.0) 

{ 

d = fz - cwv[i].z.min; 
tz = d / -b footvel.z; 

} 

else 

{ 

tz = LONG TIME; 

} 

Check for minimum value. */ 
if (tz<tmin) 

{ 

tmin = tz; 

} 

} /* end inside cwv */ 
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optimal period 

(’ompule the support phase for forward and backward gaits. * 
fs phase - legphase i BETA; 
bs phase (BET.\ - legphase|i ) BETA; 

* Compute support period. * / 
fs period — (tmin-0. 1 )/ ( 1 .0 - fs phase); 
bs period = (tmin-0.1)/(1.0 * bs phase); 

* Find the minimum support period. 
if (fs period < min fs period) 

{ ■ 

min fs period = fs period; 



if (bs period < min bs period) 

{ ■ 

min bs period = bs period; 

} 

} end support check 
} * end leg loop 

* Choose gait. * 

speed sqrt(trans rate->x * trans rale->x + 
trans rate->y trans rate->y); 

if ((speed OUTER llMIT)A'l(trans rate->x > INNER LIMIT)) 

{ 

gait FORWARD 

else if ((speed<OUTER_LlMIT)<ti-(trans rate->x< -INNER LIMIT)) 

{ 

gait = BACKWARD; 

} 

else 

{ 

No gait change. * / 

} 

if (gait == FORWARD) 

{ 

* period = min fs_period; 

} 

else 

{ 

* period — min bs period; 

} 

} end optimal period 
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This is a function for the iris 2400 program walk.c. 
decelerate. c 



Relle Lyman 04 May 1987 



^include ”gl.h” 
^include ’’device. h’’ 
^include ’’walk.h” 
ffinclude <"stdio.h> 
#include <math.h> 



decelerate(rot rate,trans rate, period, slow flag, min period) 

^ * This function computes the foot transfer rate and slows the 
vehicle if the maximum rate is exceeded. */ 



vector *rot rate, 
'’^trans rate: 



f* body rotation rate * / 

* body translation rate * / 



float ^period. * optimal period for the leg cycle */ 

*min period: /* minimum leg period * i 

int *slow_flag: flag indicating vehicle has been slowed. */ 



{ 

int i,j; 

float transfer time; j* time from liftoff to placement * ! 



if(* period < *min period) /* slow dowm "*/ 

{ 

*siow' flag = SLOW; 

^period = "^min_period; 

trans rate*>x .95; 
trans rate->y *= .7: 
trans rate->z .95: 
rot rate->x .95; 
rot rate->y *= .95; 
rot rate->z *= .7; 
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^ decelerate. c 



* display warning on screen 
pushmatrix(); 
pushviewport (); 
view port (200, 330.0. 80); 
ortho2(200.0, 330.0, 0.0. 80.0); 
color(VELLOW); 
rectfi(210,10,330,70): 
color(BLACK); 
cmov2i(2 10,30) ; 
charstr(” DECELERATION »') 
popview'port(); 
popmatrix(); 

} 

else 

{ 

*slow flag - NORMAL: 



end of decelerate 



This is a function for the iris2400 program walk.c. 
leg phase. c 



Relle Lyman 24 Aug 1986 

5^;*:+ar****5c*:^::r:r**» + ****X*^**^^***:«:»^c:»:**:»:4:**^*^*******^+**=«:*»: + 3f: 

^include "gl.h" 

^include ’’device. h'' 

^include ’’walk.h" 

^include <stdio.h> 

^include <math.h> 

leg phase( legphase, rel legphase, period) 

" Th is function computes the phase for each leg. */ 

float legphase 7 , /* phase of leg *j 

rel legphase?. relative phase of leg */ 

^period; body support period */ 



{ 

static float bodyphase = 0.0; kinematic phase of body * 
float modulus one(); modulus one function */ 

i n t i : 



Calculate new body phase. * 

bodyphase - modulus one(bodyphase + DELTA TIME/ (^period) ): 

Calculate new phase for each leg. (modi) */ 
for (i= 1; i<.7: i-^ - ) 

{ 

legphase'i = modulus one(bodyphase - rel legphase[ij); 

} 

} /* end of leg phase */ 
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This is a function for the iris2400 program v\alk.c. 
ft traj.c 



Relle Lyman 



19 Apr 1987 



+ **:t:****2f****ac*jk*!t: 



# include ”gl.h" 

^finclude "device. h” 
f include "walk.h" 
f include <stdio.h> 

^include < math.h> 

foot trajectory (leg phase, period, leg status,footpos.b^footpos,fh, 
oldfh,invh,h,cwv,trans rate, rot rate, selected gait) 

* This function calculates the trajectory for each foot. */ 

float legphase 7 . * Phase of individual leg * / 

"" period, * Optimal period * 

h 4 4 , Homogeneous transformation matrix */ 

invh 4 4 : Inverse transformation matrix * / 

vector footpos 7 . 

b footpos 7 , 

fh 7 . * Foothold selection (earth coordinates) */ 

oldfh 7 . * Old foothold selection (earth coordinates) * 

"^trans rate, * Body translation rate */ 

’^rot rate: Body rotation rate * 

vsork vol cwv 7 ; 

int leg status 7 , State of individual leg * 

*selected gait; * Desired tripod gait */ 



{ 

float trans time, * Time required to go from leg liftoff to leg touchdown 

end lift phase, /* Point in transfer phase where liftoff ends */ 
begin place phase, * Point in transfer phase where placement begins */ 
trans phase; /* Leg transfer phase * / 

static int liftoff flag,7i = OFF, * Indicates first time entering the 
subroutine in the current leg cycle. */ 
transfer_fiag[7 — OFF, 
place flag 7; = OFF; 

static vector d footpos 7 ; /* Desired foot position * 

int i; ^ Leg number * / 
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foot trajectory 

Calculate the time required to move a leg from liftoff to 
touchdown. (Transfer time) */ 
trans time - (1.0- BETA) * ABS(*period): 

/* Calculate phase points marking change of transfer mode 
(direction). Note: Modify later to account for transfer 
time. * I 

end lift phase = 0.2; 
begin place phase = 0.8; 

* For each leg */ 
for (i=l: i<7; i-^) 

{ 

Calculate transfer phase. / 

* (lift - 0.0 place - 1.0 ) 
if (* period < 0) 

{ 

trans phase = (1.0 - legphase i )^ (1.0 - BETA); 

} 

else 

{ 

trans phase = ( legphaseji - BETA) /( 1.0- BETA); 

} 



Find the leg transfer state. 
if (trans phase < 0.0) 



leg status i = SI PPORTING; 

support trajectory (liftoff flag, place flag, transfer flag,footpos, 
b footpos.invh.i); 

else if (trans phase < end lift phase) 



leg status(i| = LIFTOFF; 

lift trajectory (liftoff flag, place flag, transfer flag,footpos, 

b footpos,invh,<^trans time,&:trans phase, <lrend lift phase, 

else if (trans_phase < begin place phase) 

{ 



leg_status[ij TRANSFER FOR WARD; 

transfer trajectory (liftoff flag, place flag, transfer flag,footpos, 
b_footpos,h,invh.<^ trans time,&: trans phase, 
&:begin_place_phase,i,cwv, trans rate, rot rate,fh,oldfh, 
period, selected gait); 
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fool trajectory 

else * end place phase < irans phase < 1,0 

{ 

leg siaius i - PLACEMENT; 

placement trajectory (liftoff Hag, place Hag, transfer Hag,footpos, 
b footpos,invh,(§^trans_time,<^Urans phase,!); 



} 



end of foot trajectory * / 
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fool trajectory 






«**+ + *:*::t:*:*;4c****:^c:«c*:4i:*:*:ac:^**** / 

/ 



lift trajectory! lift off flag, place flag. transfer flag. foot pos.b footpos. 
invh,trans time.trans phase. end lift phase, leg number) 

* Th is function calculates the trajectory of the foot while it is 
being lifted from the ground. It is called from foot trajectory ( )."^/ 

vector footpos T], /* Present foot position in earth coordinates */ 

b footpos|7]; /* Present foot position in body coordinates */ 

float "*trans time, 

*end lift phase, 

*trans phase, 

invh 4][4 ; Inverse homogeneous transformation matrix */ 

int liftoff flagjl , * Indicates the first time entering subroutine 

in the current leg cycle. */ 
transfer flag 7 , 
place flag 7 j, 
leg number; 

{ 

float lift time; 
int i: 

static vector d footpos 7 ; * Desired foot position 

in earth coordinates 



i — leg number: 

* Calculate the desired footposition. 
if ( liftoff flag^i != ON) 

{ 

d_footpos[i].z = footpos[i].z + FOOTLIFTHEIGHT; 
liftoff flagiii = ON; 
transfer_flag i] = OFF; 
place flagiii = OFF; 

} 



* (Calculate the time required to reach the desired height 

from the present foot position. */ 

lift time = *trans time * (*end lift phase - *trans phase); 



143 



^ foot trajectory 



^ ('alculate the new foot position. (Earth Coordinates) * 
if (DI’^LTA TIME ^ lift time) 

fooiposii ,z — = (d footposd .z - footpos ij.z) 

" DELTA TIME / lift time; 

} 

else Last increment of time 

{ 

footposlii.z = d footpos ij.z; 

} 

* Transform to body coodinates. 

b footpos i |T = invh * |footpos[ijjT */ 
transform point(b footpos. invh.footpos.i); 

} ^ end of lift trajectory 
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foot irajectory 



/ 

transfer trajectory (liftoff flag, place flag, transfer flag.footpos. 

b footpos,h,invh. trans time,trans phase. begin place phase, 
leg number, cwv, trans rate, rot rate, fh,oldfh, period, selected gait) 



This function calculates the trajectory of the foot during the 
phase in which the foot is transfered forward. The function 
is called from foot trajectory ().* / 



vector footpos 7j, /* Present foot position in earth coordinates */ 

b footpos|7j, / * Present foot position in body coordinates * ^ 
fh 7 , / * Selected footholds (earth coordinates) */ 

oldfh 7 , Old selected footholds (earth coordinates) */ 

*trans rate. * Body translation rate * / 

*rot rate; / Body rotation rate * / 

work vol cwv 7 : * Constrained working volume in body coordinates */ 



float "^irans time, 

begin place phase, 

"^trans phase. 

* period. * Optimum period of gait */ 

h 4 4 , C" Homogeneous transformation matrix */ 
invh 4 4|; * Inverse transformation matrix */ 

int liftoff flag|7 , * Indicates the first time entering subroutine 

in current leg cycle 
transfer_flag 7j, 
place flag 7 . 

"^selected gait, Desired tripod gait ' 
leg number; 

{ 



float trans fwd 
vx,vy, 
rel hd, 
proj dist, 
min time; 



time, /* Time remaining in the transfer forward phase */ 
/* Velocity of cockpit in body coordinates */ 

/* Relative heading of cockpit velocity */ 

/* Projected distance forward for new footholds */ 

/* Minimum time to reach any cwv limit */ 



int i; 

vector cwv velocity, /* Instantaneous velocity of the center of 
the cwv (earth coodinates) * ^ 
time to limit, /* Time to reach the cwv limits */ 
bfh'7 , /* Selected foothold in body coordinates ! 

bd footposi7 : * Desired foot position in body coordinates */ 

static vector d_footpos 7 ; / * Desired foot position in earth coordinates j 
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* fool trajectory 
i - leg number; 

if ("selected gait FTL GAIT) 

{ 

if (transfer flag i != ON) 

{ 

transfer flag i = OX; 
liftoff flag i = OFF; 
place flag i = OFF; 

* Save foothold position. 
oldfh i X - fh i .x; 
oldfh i y = fh i .y; 
oldfh i .z = fh i z; 



proj dist LENGTH 0.21666; 



switch (i) 



case 1 find new left foothold * 

vx trans rate- >x; 

vy trans rate->y ^ rot rate->z " HALFLENGTH; 
rel hd atan2(vy,vx); 
bfh 1 y - 82.0; 

bfh 1 X — HALFLENGTH- proj dist*cos(rel hd)-(82.0- 
proj_dist*sin(rel_hd)) *tan(rel hd); 
bfh 1 z — -IGO.O; 

Transform to earth coordinates. 

^ fh 1 T - h * bfh i;jT */ 

transform point(fh,h,bfh, 1 ); 
break: 

case 2: find new right foothold * / 

vx trans rate->x; 

vy = trans rate->y rot rate->z * HALFLENGTH; 
rel hd = atan2(vy,vx); 
bfh~'2 .y = -82.0; 

bfh 2 X = HALFLENGTH^proj dist*cos(rel hd)-(-82.0- 
proj dist*sin(rel hd))*tan(rel hd): 
bfh 2 .z = -160.0; 

/* Transform to earth coordinates. */ 

* 'fh illT = h * bfh i'jT */ 
transform point(fh,hTfh,2); 
break; 

default . * back leg uses old front leg foothold 

fh i .X — oldfh i-2j.x; 
fh 1 .y ^ oldfh|i-2i .y; 
fh i .z = oldfh 1-2 .z; 
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foot trajectory 



* determine the desired foot position */ 
d footpos i .X fhli\x; 

d footpos i y fh|i^y; 

d footpos i .z — fh i .z; 

1 

else FWD \\ A\ E GAIT Calculate a new desired foot position 

at each time increment. */ 

{ 

Calculate the desired touchdowm point. */ 

Future change note; Change from cwv center to midstance. */ 

/* Calculate foot velocity at center of cw v (body coordinates) */ 
cw'v velocity. X = trans rate->x - rot rate*>z * cwv|i].y. center 
^ rot rate->y * cw'vjij.z. center; 
cwv velocity. y = trans rate->y ^ rot_rate->z cwv i .x. center 

- rot rate->\ * cwv[i z. center: 

cw'v velocity. z — trans rate->z - rot rate->y * cwvjij.x. center 

- rot rate- >x * cw'v i'.y. center; 

1 "^ Calculate the time to reach the limits of the cwv. * 
if (cwv velocity. X < 0.0) 

time to limit. X ( cwv i .x.min - cw v i .x. center) /cwv velocity. x; 

} 

else if (cw’\- velocity. X > 0.0) 

{ 

time to limit. X — ( cwv i .x.max - cwv|ij.x.center)/cwv velocity. x 

> 

else 

{ 

time to limit.x = LONG TIME; 

> 

if (cwv_velocity.y < 0.0) 

{ " 

time to limit. y = ( cwv il.v.min - cwv i .y. center) /cw'v velocity. v; 
} ■ ■ 

else if (cwv velocity.)’ > O.O) 

time_to_limit.v = ( cwv'i].v.max - cwv'i'.y. center) ^cwv velocity. v 

else 

{ 

time to lirnit.v = LOxNG TIME; 
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* foot irajectors 



if (cvsv velocity. z < 0.0) 

{ 

time to limit. z = ( cwv ij.z.mm - cwv i .z. center) cwv velocity. z: 

) 

else if (cwv velocitv.z > 0.0) 

time to limit. z = ( cwv ij.z.max - cwv[ij. z. center) / cwv velocitv.z 

else 

{ 

time to limit. z = LONG TIME: 



"" Determine the minimum time to reach the cwv limit. * 
min time - time to limit x: 
if (time to limit. v < min time) 

{ ’ 

min time time_to limit. y: 

} . ~ ^ / 
if (time to limit. z < min time) 

{ 

min time = time to limit. z; 

I “ 



Calculate the desired touchdown point in body coordinates. * 
* Note: This point changes if the body is in motion. * / 



bd 


footp)os 1 


.X - cwv i 


.X. center ^ cwv velocity. x * min 


time * .9: 


bd 


fool pos 1 


y — c w V i 


y. center i- cw’v velocity.) * min 


time * .9: 


bd 


footpos 1 


z — C \V V 1 


z. center ^ cwv velocity. z * min 


time .9; 



* Transform to Earth coodinates. * 

d footpos i T = h * bd footpos i]|T */ 
transform point (d_footpos,h,bd footpos, i); 

} 



* Calculate the time remaining in the transfer forward phase. * / 
irans fwd time = *trans_time * (*begin_place_phase - *trans_phase); 

Calculate the new foot position. (Earth Coordinates) * 
if (DELTA TIME < trans fwd time) 

f ~ ~ ~ 

footpos i .X -f = (d_footpos i .x - footpos[i .x) 

" DELTA TIME / trans fwd jime; 
footpos i > (d footpos ii.y - footpos i .y) 

"" DELTA TIME / trans fwd time; 
footpos ii.z = footposji .z; * Level ground assumption! * 

} 
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* foot trajectory "" 

else * Last increment of time 

{ 

footpos[i .X - d footposjij.x; 
footpos i y - d footpos|i).y: 

footposjii.z — footpos'i’.z; /* Level ground assumption! */ 

} 



Transform to body coodinates. * / 

'* ib_footpos[i)jT = invh * footpos i’ T */ 
transform^point(b foot pos,invh, foot pos,i) ; 

} /* end of transfer trajectory 
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fool trajectory 






p>lacerTieni f rajeciory(hftotf flag. place flag. transfer flag.footpos. 
b foot pos.inv h.trans time.trans phase, leg number) 

This function calculates the trajectory of the foot while it is 
being lowered from the ground. It is called from foot trajectory ().'*' 

vector footpos 7j, * Present foot position in earth coordinates */ 

b footpos|7j; Present foot position in body coordinates * 

float *trans time, 

*trans phase, 

invh 4 [4 : * Inverse homogeneous transformation matrix */ 

int liftoff flag 7 , Indicates the first time entering subroutine 
in current leg cycle */ 
transfer flag 7 . 
place flag 7 . 
leg number; 

{ 

float place time; 
int i; 

static vector d footpos! 7 ; / "*" Desired foot position in earth coordinates */ 
i — leg number. 

Calculate the desired foot position. ^ 
if ( place flag i != OX) 

{ 

d footpos i].z = footpos[ij.z - FOOTLIFTHEIGHT; 
liftoff flag i = OFF; 
transfer flag i = OFF; 
place flag i — ON; 



"" Calculate the time required to reach the desired height 
from the present foot position. * / 
place time “ *trans time * (1.0- *trans phase); 

* Calculate the new foot position. (Earth Coordinates) */ 
if (DELTA TIME < place time) 

{ 

footpos i .z (d footpos i .z - footpos i .z) 

* DELTA TIME / place time, 

} 
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! foot trajectory * 

else ' Last increment of time 

{ 

footpos i .z - d footpos[iLz; 

} 



* Transform to body coodinates. * 

/* b footpos ij T = invh * 'footpos i] T 
transform point(b footpos, invh, footpos, i); 

} /* end of placement trajectory * / 



foot trajectory 



* * 






x + + **»*=»:aF::T.*:*c**xx*xxx**TX + *xxx*>R*:^ + ^***»x + * + x** + + 



support trajectory (liftoff tiag, place Hag, transfer flag. foot pos, 
b foot pos. invh, leg number) 

* This function calculates the trajectory of the foot during the 
foot support phase. It is called from foot trajectory ().*/ 

vector foot pos 7 , /* Present foot position in earth coordinates ^ ■ 

b foot pos 7 ; /* Present foot position in body coordinates * / 

Hoat invh 4 4 ; Inverse homogeneous transformation matrix 

int liftoff flag 7 , * Indicates the first time entering subroutine 

in current leg cycle ^ 
transfer flag 7 , 
place flag 7 , 
leg number; 

int i; 



In this phase the foot is kept stationary on the ground. It 
is assumed that the foot will not slip or move accidently. 

i - leg number. 

Transform foot position to body coodinates. * 

^ b foot pos i T — invh * footpos i T * 
transform point (b foot pos, invh. footpos, i); 

* Turn off flags. ^ t 
liftoff flag 1 = OFF; 
transfer flag i — OFF; 
place flag i = OFF; 

} / end of support_trajectory */ 
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This function is for the program walk.c on the iris-2400, 
conwalk.c 



Based in part on J.H. Kessler’s 
R L. Lyman program "conwalker.c’* 

24 Apr 1987 

/ 



=^include "gLh" 

^include ’’device. h” 
include "walk.h" 

/ * This function calls up the walker from constructw^alker (wdth legs 
already properly positioned) and then rotates and translates it as 
commanded. ! 

Note; Due to the limited number of bit planes available 
four separate walkers are constructed, one for each viewing 
quadrant. The walker for each quadrant is drawn from furthest 
component to nearest. This provides a quasi- Z buffer effect 
while in double buffer mode. */ 



makewalker(machineobject,d 1 ,d 2, theta, knee, gamma, alpha. transrot tag, 
tr end tag,w alker,leg.thighobj,actuatorobj,shinobj, 
legmovetag,thighmovelag.actmovetag,shinmovetag ,tx,ty,tz, 
rolLelev,azimuth.hx.hy,hz,l4) 

Tag transrot_tag 4 ,tr end tag 4 ,legmovetag 4 , 

thighmovetag|];2 .4 ,actmovetag[];2 4 ,shinmovetag ' 2 4'; 

Object machineobject ;4;,walker[4j,thighobJi]j2 ;4],actuatorobj|][2]l4), 
shinobj|jj2) 4j,leg[][4 ; 

int dl j,d2[j,knee[j 2^ ; 

Angle theta[j,alpha ,gamma[]; 

float tx,ty,tz,roll,azimuth,elev, 
hx 7phy|7j,hz|7],l4|7]: 



{ 

int n; 
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conwalk.c 



conslructwalker(v\ alker.d 1 ,ci2.knee.alpha.gamma.thei a.leg,t highobj, 
ac t u at orobj, sh inobj. leginovet ag, thigh movet ag. acimo vet ag. 
shinmovetag,hx.hy,hz,l4) ; 

for (n -0; n<.4: — ) * Rotate and translate the walkers in each 

quadrant. * 

{ 

machineobject n| — genobj(); 
makeobj(machineobject n ); 
pushmatrix() ; 

* Note: Each walker is built on the origin. Rotations are done 
before translating to the proper location. */ 
transrot tag n =gentag(); 
maketag(transrot tag n ); 

translate(tx.ty,tz ); 
rotate((int) (elev "" 573), ’Y'); 
rotatej(int) (roll * 573), ’X’); 
rotaie((ini) (azimuth * 573). ’Z’); 

tr end tag n -gentag(); 
maketag(tr end tag n ); 

callobj(walker n ); 

popmatrix() : 
closeobjO; 

} * end quadrant loop * ^ 

} end of rnakewalker * 
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/ conwalk.c 



makeground(groundobject) 

/ This function creates a checkerboard groundplane below the ASX object/ ^ 
Object *groundobject: 

{ 

Object squareobject; 

Tag transqrtag; 

static int 

groundl(4;i3 -{{ 1000, -500,0}, {1000, 500,0}, {-1000, 500,0}, {-1000, -500,0} }, 
ground2j4][3={ {2000, -1000,0}, {2000, 1000,0}, {-2000, 1000,0}, {-2000, -1000,0}}, 
squarel4'|3l=:{ {0,-100,0}, {0,0,0}, {-100, 0.0}, {-100,-100,0}}; 

int i,j; 

float tx.ty; 

squareobject genobj(); 
makeobj (squareobject ) : 
color(WHITE): 
polfl(4, square): 
closeobj'O; 

*groundobject=genobj(); 
makeobj (^groundobject); 

color(RED); fill outer background squares */ 

polfi(4,ground2); 

color( GREEN); /* fill inner background squares 
polfi(4,groundl); 

for (i— 0; i<40: i^-l-) 

{ 

for (j = 0; j<20; j-^ + ) 

{ 

if ((i^j)%2 < 1) 

{ 

tx=(i-20)*(-100.0); 

ty=(j-10)*(-100.0); 

pushmatrix(); 

translate(tx,ty,0.0); 

callobj(squareobject); /* place the w^hite squares */ 
popmatrix(): 

} end if */ 

} / * end for j * / 

} / * end for i */ 
closeobj(): 

} /* end makeground * / 
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con walk. c 






constructs alker(w alker,dl, d2. knee, alpha, gamma, theta, leg, thighobj, 
actuatorobj,shinobj,legmovetag,thighmovetag,actmovetag, 
shin move tag,hx.hy,hz, 14) 

This is w here the walker is made. Here each part is assembled 
and then the parts are put together. This assembled walker is 
then rotated and translated in makewalker which is called by 
the main program. * j 

Tag legmovetag[]|4 ,thighmovetag 2'U ,actmovetag[]|2 [4 , 
shinmovetag,] 2; 4j; 

Object walker 4 .leg j4 .thighobj' J^2 4 .actuatorobj; 2 4 ,shinobj[l 2 4 ; 

int dl .d2 .knee j|2 ; 

Angle alpha ,gamma[ .theta ;; 

float h.x 7 .hy 7|,hz[7 , leg pivot position * 

14 7 : 



{ 

Object body.head.eye.bo.xobj 7 •. 

static float legx 7 — {0.0.153.0.155.0,0.0,0.0,-155.0,-155.0}, 
legy 7 ={0.0.82 0,-82.0.82.0,-82.0,82.0,-82.0}, 
legz,7 {0.0.0.0.0.0,0.0,0.0,0.0,00}; 

Coord x,y.z ; 

int i,j,k.n,legnum ; 

static int 

/* Coordinates for building the body of the asv */ 

blackbodyU'l3j = {{206,50.22}, {206, -50.22}, {206.-30,-101}, {206,30, -101}}, 
lbodyarry;4 :3]={ {-200, 30,-10l},{-200. 50. 22},{206,50, 22}. {206, 30,-101}}, 
rbodyarry 4 |3i = { {-200,-30,-101}, {206,-30,-101}, {206, -50,22}, {-200.-50, 22}}, 
tbodyarry,4:|3={ {-200,50,22}, {-200, -50, 22}, {206,-50,22}, {206,50,22}}, 
bbodyarry!4 3 ={{206,-30, -101 }, {-200, -30,- 101 },{-200,30.- 101 },{206.30,-10l}}, 
backbodyarry 4 3 ={{-200, 30,-101}, {-200, -30, -101}, {-200, -50,22}, {-200,50,22}}, 
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conwalk.c 



/* Coordinates for building the hydraulics housing structure * 

front rt Jop 4 3 :^{{27,-25.16}.{38,-25,-13},{38,-13,-13}.{27,-13,16}}, 

front rt bttm 4 ,(3: {38,-25,- 13}, { 38, -25.-46}, { 38, -13,-46}, { 38.-13.-13 } }, 

rt interior 5' 3 j = {{ 20.-25,38} ,{ 38,-25.- 1 3 },{ 38.-25.-46 }.{ -38.-25.-46 }, {-38,-25,38} }. 

rt_sidel5 3 ={ {-38.-25, 38},{-38,-25,-46},{ 38, -25,-46},{38,-25.-l3}.{ 20,-25,38} }, 

lt_interior;5 3 ={{-38, 25, 38}. {-38, 25.-46}, {38, 25,-46}, {38, 25,-13}, {20,25, 38}}, 

It_sidel5||3) = {{ 20.25, 38}, {38,25,-1 3}, {38.25.-46}, {-38, 25.-46}, {-38. 25. 38}}, 

top_boxi4|l3 ={ {20,-25,38}, {20,25, 38}, {-38, 25.38}, {-38.-25. 38,}}, 

back_box 4: '3 ={{-38,25,-46}, {-38,-25,-46}, {-38,-25.38}, {-38, 25, 38}}, 

front top 4 [3 ={{ 20.25.38}, { 20,-25,38 },{ 27,-25,13} ,{ 27.25. 13 } }, 

front Jt_topi4l 13) = {{ 27. 13, 16}, {38, 13,- 13}, {38.25,-13}. {27.25. 16}}, 

frontJt_bttm 4 1;3| = {{ 38,13,-13}, {38, 13.-46},{38,25.-46},{ 38,25.-13}}, 

bttm_lt[4'|3| = {{38,25,-46},{38,13,-46},{-38,13,-46},{-38,25,-46}}, 

bum rt(4) 3 ={{38.-25,-46}, {38,- 13,-46}, {-38,-13,-46}, {-38,-25,-46}}, 

highbox_top!4l!3; = {{-8,-25,88},{8,-25,88},{8,25,88},{-8,25,88}}. 
highboxJrontl4]l3)={ {8, 25,88}, {8,-25, 88}, { 10.-25, 38}, { 10,25,38}}. 
highbox back4]|3!={{-8,-25,88}.{-8.25,88},{-10,25,38},{-10,-25,38}}, 
highbox_rt 4 [3' = {{ 8.-25, 88 },{ -8.-25. 88}, {-10,-25, 38},{ 10,-25, 38}}. 
highbox JtU 3 - { {-8.25.88},{8.25.88}.{10.25,38},{-10.25,38}}, 

rt spar frontil |3 ={{ 79.- 13.-20}. { 79.-25.-20},{ 79.-25,-30}, {79,-13,-30} }, 
rt spar lop 4 3 = {{79.-13.-20}, {38. -13.-19}. {38,-25, -19}, {79,-25,-20}}, 
rt’ spar bltm 4 ,3 { {38.-13,-32} .{ 79.-13.-30},{ 79.-25,-30}, {38,-25,-32} }, 

rt spar jt 4 3 ={ {38,-25,-32} ,{ 79,-25.-30},{ 79.-25.-20} ,{ 38,-25,-19} }. 
rt spar It 4 3i = {{79.-13,-30}, {38,-13,-32}. {38,-13,-19}, {79,-13, -20}}, 

It spar front 4 ;3j={ { 79.25,-20}, {79. 13,-20 },{ 79, 1 3,-30}, { 79,25,-30} }, 

It sparjop 4) 3)={ {79,25.-20}, {38, 25,-19}, {38, 13,-19}, {79, 13,-20}}, 

It spar_bttm[4]|3]={ {38.25,-32}. {79, 25,-30}, {79, 13.-30}, {38,13,-32}}, 
It3par_rt4);3! = { {38, 13,-32}, {79, 13, -30}, {79,13,- 20}, {38, 13,-19}}, 
lt_spar_lti4|[3] = { {79. 25,-30}, {38,25,-32}, {38,25,-19}, {79.25.-20}}, 

j* cab construction arrays */ 

cab_bottom’4|(3={ {305,-30,-101}. {206,-30,-101 },{206, 30,-101}, {305, 30,-101}}, 
cab top 4 3 ={{250.33.74}, {206,33,74}, {206.-33, 74}, {250,-33. 74}}, 

cab_fwd_supporti4l[3 -{ {305,30,-101}, {305, 41, -16}, {305.-41,-16}, {305,-30,-101}}, 
cab fwd Joweri4|,3' = {{ 305.4 1,-16},{3 18,48. 8 },{ 318,-48. 8},{ 305.-4 1.-16}}, 
cab fwd_upper|4]|3]={ {318,48.8}. {302, 33,68}, {302.-33,68},{318.-48, 8}}, 
cab_fwd_ovhd|4i|3 ={ {275, 33,68}, {250,33, 74}. {250.-33, 74}. {275,-33, 68}}, 

cab rt_support[4 '3] = { {305 .-30,-101 },{305,-4 1,-16 },{ 206,-41. -16}, {206,-30, -101}}, 
cab rt_lower'4 ,3 ={{305,-4 1.-16 },{ 318,-48. 8}, {206.-48, 8}, {206,-4 1.-16}}, 
cab rt upper^4j|3) = { {318,-48, 8}, {302,-33, 68}. {206,-33,68}, {206.-48. 8}}. 
cab_rt_ovhd;4][3] = {{275,-33,68},{250,-33,74},{206.-33, 74}, {206.-33, 68}}, 
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* conwalk c 

cab It support 4 3 {{ 206,30.- 101 },{ 206,4 1 16} ,■{ 305,4 1 16} ,{ 305,30.- iOl }} , 

cab"lt lower 4 3 {{206.41,-16}, {206, 48, 8}, {318. 48, 8}, {305, 41, -16}}, 

cab_lt_upper 4 3 { {206.48.8}, {206.33, 68}, {302, 33,68}, {318.48. 8}}, 

cab It ovhd 4 3 ={{ 206.33.68 },{206,33. 74 },{250, 33.74 } ,{275,33,68} }, 

cab_aft_support 4 3: = { { 206.-30,-101 },{ 206,-4 1 ,-16} ,{206,41,-16} ,{206,30,- 101 }} , 
cab aft lowers 3 ={ {206,-41,- !6},{206,-48, 8}, {206.48, 8} ,{ 206,4 1 ,- 16} } , 
cab_afi_upper 4 3|={{206,-48,8},{206.-33.68},{206,33,68},{206.48,8}}, 
cab aft ovhdU 3)={ {206,-33,68}, {206,-33, 74}, {206, 33, 74}, {206,33, 68}}, 

scanner fwd lower 4' 3i = {{302.33, 68},{322, 33,95}, (322,-33,95},{302,-33, 68}}, 
scanner_fwd_upperl4il3: = {{322.33,95},{322,33,10l},{322,-33,10l},{322,-33,95}}, 
scanner rt 5;!3 ={ {302,-33,68}, {322,-33, 95},{322,-33,101 },{275,-33,101 }, {275,-33, 68} }. 
scanner 1t:5i 3 = {{302. 33,68 },{275, 33,68}, {275, 33, 101 },{ 322, 33, 101 },{322, 33,95 } }, 
scanner_aft 4 3 = {{ 275,33,101 },{275,33.68},{275,-33,68},{275,-33,101 } }, 
scannerjop 4 3, = { {322.33,101 },{275, 33. 101 },{ 275,-33, 101 },{ 322,-33. 101 } }; 

* The making of the leg is quite complicated. Each leg consists of an 
upper link (thigh), lower link (actuator), and a shank (shin). These 
‘egmenls are first defined in a standardized orientation, and are then 
rotated and translated into the proper position. This is done by using 
2 objects for each segment. The first object is the correctly rotated 
segment, and the second object is the correctly translated first 
object. Thus the segment is then in the proper position. To hold the 
screen coordinate system fixed the matrix is pushed before each translation 
or rotation and then popped after the object is constructed or called */ 

for (n=0; n< 4; n-^^) * Make a set of legs for each viewing quadrant 

Each quadrant must have unique tags. * j 

{ 

for(legnum = l ;legnum<7;legnum'^^) 

{ 

* Each segment is constructed and positioned */ 
buildthigh (n,legnu m,dl, alpha, thighobj,thighmovetag) ; 
bu ild act u ator ( n, legn um, d2, alpha, act u at orobj,actmovetag) ; 
bu i Idsh in (n.legnum, knee, gamma, shinobj, shin movetag) ; 

leg legnumjjn , — genobjO; 
makeobj(leg(legnumj|nj); 
pushmatrix(); 

* translate(legx|legnum|,legy legnum ,legz legnum ) ; 
t ran slate (hx[ legn um,,hy! legn um .hz legn um ) ; 

legmovetag legnum n ~gentag(): The leg is assembled from 

makelag(legmovetag|legnum nj); its parts and the entire leg is ^ j 
then rotated to the proper angle. */ 
rotate(theta legnum ,'X'); 

translate(0. 0,14 legnum , 0.0); / * extend leg outward * 
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conv\ alk.c 



if ({(n > 1 )(§^<^'(legnum < 5)) 

((n < 2)(L^&r(legnuni > 4))) * Build the left side first. * 

if (legnum >4) * Reverse the back legs. */ 

{ 

pushmatrix( ); 
rotate( 1800,'Z’); 

} 

color(BL ACK): 
polfi(5,lt interior); 

color(GREEN): 
polfi(5,lt side); 
polfi(4,front_lt top); 
polfi(4. front It bttm); 
polfi(4,bttm It); 

polfi(4,lt spar front); 
polfi(4,it spar bttm); 
polfi(4,lt spar It); 
polfi(4dt spar rt); 

color(BLUE); 
polfi(4,lt spar top); 

color(BLACK); 
polyi(4,lt spar rt); 

color (CYAN) ; 

callobj(thighobj I legnum l’ n]); 
callobj(actuatorobj legnum (l jn ); 
callobj(shinobj legnum I n]); 

color(GREEN) ; 
polfi(4,rt _spar front); 
polfi(4,rt_spar_bttm); 
polfi(4,rt spar It); 
polfi(4,rt spar rt); 

color(BLUE); 
polfi(4,rt spar top); 

color(GREEN); 
polfi{4, front rt bttm); 
polfi(4, front rt top); 
polfi(4, front top); 
polfi(4,bttm_rt); 
polfi(4,back box); 
polfi(4,top box); 
polfi(5,rt side); 
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rolor(BLACK); 
polyi(‘1 top box); 

[)(»l\i(S,rt side); 
polyi(4.ri spar rt): 

color(GREEN); 
polfi(4,highbox_front); 
polfi(4,highbox_lt); 
polfi(4,highbox back); 
polfi(4,highbox_rt); 
polfi(4,highbox top); 

color(BLACK); 
polyi(4,highbox top); 
polyi(4.highbox rt); 
poly i( 4. high box back); 

if (legnurn > 4) * For reversing the back legs. 

{ 

poprnatrixO ; 

} 

} 

else * Build the right side first. 

{ 

if (legnurn 4) Reverse the back legs. ^ j 

{ 

pushmatrix( ) ; 
rotate(l800.'Z’); 

} 



color(BL ACK); 
polfi(5,rt interior); 

color(GREEN); 
polfi(5,rt side); 
polfi(4,front_rt_top); 
polfi(4, front rt_bttm); 
polfi(4,bttm_rt); 

polfi(4.rt spar burn); 
polfi(4,rt spar rt); 
polfi(4,rt spar It); 
polfi(4.rt spar front); 

color(BLUE); 
polfi(4.ri spar top); 
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color(BL ACK): 

polyi(4,rt spar It); 

color(C\ AX) ; 

callobj(thighobj legnum 1: n ); 
callobj(actuatorobjllegnum) 1 \ n^); 
callobj(shinobj|legnuni 1; n ); 

color(GREEN) ; 
polfi(4,back box): 
polfi(4.bttm It); 
polfi(4, front top); 
polfi(4,fronl It bttm); 
polfi(4, front It top); 

polfi(4.1t spar bttm); 
polfi(4,lt spar rt); 
polfi(4,lt spar It); 
polfi(4,lt_spar front); 

color(BLUE); 

polfi(4,lt spar top); 

color(GREEN); 
polfi(4.top box); 
polfi(5,lt side); 

color(BL ACK); 
polyi(4,top box); 
polyi(5,lt_side); 
polyi(4,lt spar It); 

color(GREEN); 
polfi(4,highbox back); 
polfi(4.highbox_rt); 
polfi(4,highbox front); 
polfi (4,highbox It); 
polfi (4,highbox top); 

color(BLACK); 
polyi(4,highbox top); 
polyi(4,highbox_lt); 
polyi(4,highbox_front); 
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if (legnum > 4) ’ For reversing the back legs. * 

{ 

popmatrix() ; 



popniatrix(); 
closeobjO : 

} / * end of leg loop */ 

} * end of quadrant loop ""I 

body^genobj 0 ; * The body is constructed * 

rnakeobj(body) ; 

color(LTYELLOW) ; 
polfi(4.lbodyarry) ; 
polfi (4,backbodyarry) ; 
polfi(4,bbodyarry) ; 
polfi(4.rbodyarry ) ; 

color(\ FLLOW); 
polfi(4.tbodyarry ); 

color(BLAC’K) : 
polfi(4.blackbody) ; 
closeobjO ; 

head — genobj( ) : * construct the head * 

makeobj(head) : 
color(YELLOW); 
polfi(4,cab top); 
polfi(4,cab_fwd_o\ hd); 
polfi(4,cab rt ovhd); 
polfi(4,cab It ovhd); 
polfi(4,cab aft ovhd); 

color(BLACK); 
polfi(4,cab bottom); 
polfi(4,cab fwd support); 
polfi(4,cab rt support); 
polfi(4,cab It support); 
polfi(4,cab aft support); 

color(WHlTEl); 
polfi(l,cab fwd lower); 
polfi(4,cab rt lower); 
polfi (4. cab It lower); 
polfi(4,cab aft lower); 
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color(WHlTE) ; 
polfi(4,cab fwd upper); 
polfi(4,cab rt upper): 
polfi(4,cab It upper); 
polfi(4,cab aft upper): 

color(BLACK) ; 
polfi(4,cab top); 
polyi(4,cab fwd lower); 
polyi(4.cab fwd upper); 
polyi(4.cab fwd ovhd); 
polyi(4.cab rt low^er); 
polyi(4,cab_rt upper); 
polyi(4,cab rt ovhd); 
polyi(4,cab It lower); 
polyi(4,cab_lt_upper); 
polyi(4,cab It ovhd); 
closeobjO ; 

eye— genobjO ; * contruct the radar (eye)* 

makeobj(eye) ; 
color(RED): 

polfi(4, scanner fwd upper); 
polfi (4. scanner fwd lower); 
polfi(5, scanner rt): 
polfi(5. scanner It); 
polfi(4, scanner aft): 

color(BLACK) ; 
polfi(4. scanner top); 

color(BLUE) ; 
closeobjO : 

walker jOj~genobj ( ); /* assemble all the parts for quad I */ 

makeobj(walkerjOj); * back and right first */ 

callobj(legi6'l0 ); 
callobj(legi4![0 ); 
callobj(leg|2!io ); 
callobj(body); 
callobj(head); 
callobj(eye); 
callobj(leg|5;[0 ); 
callobj(leg|3 0 ); 
callobj(leg; 1 0); 
closeobjO ' 
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walker 1 ~genobj(): 
makeobj( walker 1 ); 
callobj(leg 2' 1 ); 
callobj(leg 4^ 1 ); 
callobj(leg 6] 1 ): 
callobj(head); 
callobj(e\ e); 
callobj(bod> ); 
callobj(leg 11): 
callobj(leg 3 1 ): 
callobj(leg 5, 1 ); 


assemble all the parts for quad 11 * 
front and right first 


walker 2 -genobj(); 
ni a keobj (walker 2 ): 
callobj(leg 1 i!2 ): 
callobj(leg 3]|2 ): 
callobj(leg 5| 2 ); 
callobj(head); 
callobj(eye) ; 
callobj(bod\ ); 
callobj(leg 2 2 ); 
callobj(leg 4^ 2 ); 
callobj(leg 6] 2 ); 


assemble all the parts for quad III* 
front and left first * 


walker 3 -genobj(): 
makeobj (walker 3 ); 
callobj(leg 5 3 ): 
callobj(leg 3 3 ): 
callobj(leg 13): 
callobj ( bod\ ) : 
callobj(head) : 
callobj(eye) : 
callobj (leg 6](3 ); 
callobj(leg 4 Is!); 
callobJIleg 2]|3i); 


* assemble all the parts for quad IV 

* back and left first */ 
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b u ildthig h (n,legnum,dl, alpha. thighobj .thighmovetag) 

this function constructs the thigh (upper link) and rotates, then 
translates it into the proper position * / 

Tag thighmovetag ji‘2 4|; 
int ndegnum.dl j ; 

Angle alpha ; 

Object thighobj 2| 4 ; 

{ 

static int 

thighltside -{ {0,10, 7}, { 102,10, 7},{ 102, 10,-7 }.{0, 10,-7} }, 

thighrtside 4 3 ={ {0,-10, -7}, { 102,-10,-7 },{ 102, -10,7}, {0,-10,7} }, 
thighfrontj4 :3] = { {0,-10, 7}, {102, -10, 7},{ 102, 10,7}, {0,10,7}} , 
thighbttmi4 ;3) = { {0,10, -7},{ 102, 10,-7 },{ 102,-10,-7}, {0.-10,-7}}; 



thighobjilegnum () n =genobj(); 
makeobj (thighobj legn urn: (O; n]); 

pushmatrix() : 

thighmovetag legnum 0 ^n -gentag(): /* rotate thigh * / 

maketagi thighmovetag legnum 0 n! ); 
rotate(alphadegnum!,’Y') ; 

if(legnum 4 ) * Build the left side first. 

{ 

color(CY AN): 
polfi(4,thighbttm); 
poIfi(4,thighltside); 
polfi (4, thighrtside); 

color(RED); 

polfi(4,thighfront); 

color(BLACK); 
poly i (4, thighrtside); 

} 
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else * Build the right side first. */ 

{ 

color(C\ AN); 
polfi(4,thighbttm); 
polfi(4.thighrtside); 
polfi(4,thighltside) ; 

color(RED) ; 
polfi(4,thighfront); 

color(BL ACK); 

polvi(4,thighltside): 



popmatrix() ; 
closeobj'O : 

t highobjjlegnum 1 n — genobj() ; 
makeobj(thighobj legnum In); 

pushmatrix( ) : 

thighmovetag legnum! 1 n — gentagf); 

makelag(thighmovelag legnum | lj| nj; translate thigh 

translate(0.0,0.0, (float) (-d 1 legnum )) ; 

callobj(thighof>j legnum’[0 nj); 
popmatrix() : 

closeobjO ; 
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buildact uator(n, legnum,d2, alpha, act uatorobj, act movetag) 

* construct the actuator (lower link) * ^ 

Tag actmovetag; 2l[4|; 

int n,legnum,d2 |; 

Angle alpha}]; 

Object actuatorobj’]|2][4 ; 

{ 

static int actltside(4][3j = { {0,10,5},{83,10,5},{83,10,-5},{0,10,-5}). 
actfronti4 3 ={{0,-10, 5}. {83, -10, 5}, {83, 10,5}, {0,10, 5}} , 
actrtside'4 :|3! = { {0,-10, -5}, {83,-10,-5}, {83. -10,5}, {0,. 10,5}}, 
actbttm 4 3 ={{0,10, -5}, {83, 10,-5}, {83, -10, -5}, (0,-10, -5}} ; 



actuatorobj legnum O n ~genobj(); 
makeobj(actuatorobj legnuml[0l'n ); 

pushmatri\( ); 

act movetag legnum 0 |n =gentag(); 

maketag(actmovetag;legnum 0][n|); /* rotate actuator * 

rotate(alpha legnum], Y’) ; 

if(legnum>4) Build the left side first. 

{ 

color(CY AN) ; 
polfi(4.actbttm); 
polfi(4,actltside); 
polfi(4,actrtside); 

color(RED); 

polfi(4,actfront): 

color(BLACK); 

polvi(4,actrtside); 

} 
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else /* Build the right side first. 

< 

color(CV.\\); 

polfi(4,actbttm); 

polfi(4,actrtside); 

polfi(4,actltside); 

color( RED); 
polfi(4,actfront); 

color(BLACK); 

polvi(4,actltside); 

} 

popmatrix(); 

closeobjO; 

act uatorobj, legnum 1 n genobj(): 
makeobj(act uatorobj legnum jl n ): 

pushrnatrix(): 

actmovetag legnum 1 [n -gentag(); 

maketag(actmovetag legnum]! 1 In ); translate actuator */ 

translate! ( float )(d2 legnum ), 0.0, (float) (-L3)); 
callobj(actuatorobj legnum O’ n ): 
popmatrix( ): 
closeobjO; 

"" end of buildactuator * 
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buildshin(n,legnum,knee,gamina,shinobj,shinmovetag) 

construct the shank (shin ) */ 

Tag shinmovetag ]|2]!4 ; 
int n,legnum,knee^ j 2.; 

Angle gammaij; 

Object shinobj[l 2 4; 

{ 

static int 

shinltside!6l,3| = {{6,5.3},{l0,5,-59},{-7,5,-50},{-6,5,3}.{-3,5.6},{3.5,6}}, 

shankltside 4]*3] = {{10,5,-59},{-23,5,-102},{-36,5,-100M-7,5,-50}} , 

shinfront 4]l3>{{6,5,3},{6,-5,3},{ 10.-5, -59}, { 10,5,-59} }, 

shankfront[4 '3l = {{l0,-5,-59},{-23,-5,-102},{-23.5,-102}.{7,5,-59}}, 

ankleltside 6 3! = {{-23.5,-l02},{3,5,-153},{2,5,-157},{-3,5,-158},{-6.5.-158},{-36,5,-100} }, 

shinrtside 6l 3 —{ {3, -5, 6}, {-3, -5, 6}. {-6, -5, 3}, {-7, -5,-50}, { 10, -5. -60}, {6, -5, 3}}, 

shankrtside[4M3j = {{-7,-5,-50},{-36,-5,-100},{-23,-5,-102},{l0,-5,-59}} , 

anklertside 6i 3j = {{-36,-5,-100},{-6,-5,-158},{.3.-5.-158},{2,-5,-157},{3,-5,-153},{-23,-5,-102}}. 

anklefront 4 3| = {{-23,5.-102},{-23,-5,-102},{3,-5.-153},{3,5,-!53}}. 

shinback;4][3] = { {-7,-5,-50} ,{-6,-5,3} ,{-6,5,3},{-7,5,-50} } , 

shankback[4 |3| = { {-36,-5,-l00},{-7,-5,-50},{-7.5,-50},{-36,5.-l00}}, 

ankleback;4'^3] = {{-6,-5,-158},{-36,-5,-100},{-36,5,-100},{-6,5,-!58}}, 

bottomjwd 4 3! = {{3,5,-153},{2, 5,-157},{2,-5,-!57},{3,-5,- 153} }, 

bottomjnid 4 3: = {{2.5,-157},{-3,5,-158},{-3,-5,-158},{2,-5,-157}}, 

bottom_aft[4][3]-{{-3,5,-158},{-6,5,-158},{-6,-5,-158},{-3,-5,-158}}; 

shinobJ[legnum][0j[n| — genobjO; 
makeobj(shinobj|legnum |0 n ); 
pushmatrix() ; 

shinmovetag legnum]|0 n]=gentag(); 

maketag(shinmovetag'legnum][0][n]); /* rotate shank */ 
rotate(gamma legnum ,’Y’); 

if(legnum>4) /* Build the left side first. */ 

{ 

color(BLACK); 
polfi(4, bottom fwd); 
polfi(4, bottom mid); 
polfi(4, bottom aft); 

color(CYAN); 
polfi(4,ankleback); 
polfi( 6, ankleltside); 
polfi (6, anklertside); 

color(RED); 
polfi(4, anklefront) ; 
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color(CYAN); 
polfi(4.shankback) ; 
poHi(4,shankltside); 
polfi(4.shankrtside); 

color( RED); 
polfi(4.shankfront) ; 

color(CYAN): 

polfi(4.shinback); 

polfi(6,shinltside); 

polfi(6.shinrtside); 

color( RED) ; 
polfi(4.shinfront) ; 

color (BLACK): 
pol\ i(6,anklertside); 
polyi(4.shankrtside) ; 
pol\ i(6.shinrtside): 

else Build the right side first. */ 

{ 

color(BLACK); 
polfi(4. bottom fwd) ; 
polfi(4, bottom mid) ; 
polfi(4, bottom aft) ; 

color(CY AN) ; 
polfi(4,ankleback); 
polfi(6.anklertside); 
polfi(6.anklekside); 

color(RED); 

polfi(4.anklefront); 

color(CYAN); 
polfi(4,shankback) ; 
polfi(4,shankrtside); 
polfi(4,shankltside); 

color(RED); 

polfi(4,shankfront); 

color(CYAN); 
polfi (4.shinback); 
polfi(6.shinrtside); 
polfi (6,shinltside); 
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color(RED): 

polfi(4,shinfront): 

color(BLACK): 
pol> i(6,anklelt side); 
poly i( 4, sh an kit side); 
polvi(6,shinltside); 

} 

color(BLACK) ; 

pushmatrix(); 
rotate(“900,'X’); 
translate (0.0, 0.0, 5.0); 
circf(0. 0,0. 0,7.0) ; 
circf(0.0,32. 0,5.0) : 
popmatrix() ; 

pushmatrix(); 
rotate(900.’X’): 
t ranslate{ 0.0.0. 0,5.0) ; 
circf(0. 0,0. 0,7.0) ; 
circf(0.0.-32.0.5.0) ; 
poprnatrix(); 

popmatnx(); 
closeobj( ); 

shinobj legnum l|(nj = genobj(); 
makeobj(shinobj|legnumj Ijin ); 

pushmatrix(); 

shinrnovetag I legnum 'l](n) = gentag(); 

maketag(shinmovetag legnum 'l][n]); /* translate shank */ 

translate((float)knee[legnum] jO), 0.0, (float) knee I legnum] [l ) ; 

callobj (shinobj 1 legnum ||0j[n): 

popmatrix(); 

closeobj'O; 



} * end of buildshin * / 
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This IS a function for the iris240() program walk.c. 
tool box. c 



Relle Lyman 25 Aug 1986 

/ 

^include "gl.h’' 

^include ’’device. h” 

^include ’’walk.h” 

^include < stdio.h> 

^include < math.h> 

a- + x + *xx*x^**:+:5fc:«:+:*xi*x:*;x**xx*tx*x***********x***:4r**^**x*t**x*x*x:t:*****:^***;*** j 

transform point (p2,m, pi. i) 

This function changes the coordinate system for a point vector 
using a homogeneous transformation submatrix. p2 = m * pi * ^ 

int i; , * Leg number * 

fioat m 4 4i; * Homogeneous transformation submatrix */ 

vector pi 7 . Vector represented in first coordinate system */ 

p2 7 : * Vector represented in transformed coordinate system * 

{ 

p2 i .X = m|0l[0 *pl i;.x — m 0^ 1 *pTi .y -r m'O] 2 *pl[i'.z — m|0]|3b 

p2 i y = m|l [O *pi i .x - m 1 1 *pl i .y ^ m li 2 *pl(i .z ^ m[lj|3’; 

p2 i .z — m 2 0|*pl iLx - m 2 1 *pl i’.y - tn(2j[2 *pl ij.z m 2! 3 : 

} , end of transform point * 

^5fc*x*x + x**x*x*x* + *x*xx* + x*x*x*x*x*l:**xx*^*x*x**x***x*x*x****xx** + x**xx*x**x*^ / 

/ 

float modulus one(temp) 

This function performs the modulus one operation on numbers of type float 

float temp; 

{ 

while (temp >= 1.0) 

{ 

temp 1.0; 

while (temp < 0.0) 

{ 

temp 1.0; 

} 

return temp; 

} * end of modulus one */ 
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Makefile 

# This is Makefile. It is used in the utility make to speed 

# compilation of w alk.c. To use it, just type ’’make’f 

CFLAGS = -Zf-Zg -g 



SRCS = walk.c 
con walk.c 
support. c 
tool box. c 
steering. c 
body rates. c 
ft traj.c 
opt period. c 
leg phase. c 
con work vol.c 
driver. c 
status. c 
decelerate. c 
init.c 



OBJS - walk.o 
conwalk.o 
support. o 
toolbox, o 
steering. o 
body rates. o 
ft traj.o 
opt period. o 
leg phase. o 
con work vol.o 
driver. o 
status. o 
decelerate. o 
init.o 

walk : (OBJS) 

cc -o walk (OBJS) -Zg -Zf 

(OBJS) : walk.h 
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